Examples with KKTSolver maspack.solvers.KKTSolver used on opensource projects

Example 1 with KKTSolver

use of maspack.solvers.KKTSolver in project artisynth_core by artisynth.

the class MechSystemSolver method computeRigidBodyPosCorrections.

protected boolean computeRigidBodyPosCorrections(VectorNd pos, double t) {
    // assumes that updateMassMatrix() has been called
    int velSize = myActiveVelSize;
    if (velSize == 0) {
        return false;
    }
    if (myConSolver == null) {
        myConSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    updateUnilateralConstraints();
    myVel.setSize(velSize);
    if (myGsize > 0 || myNsize > 0) {
        // mySys.getBilateralOffsets (myRg, myBg, 0, MechSystem.POSITION_MODE);
        mySys.getBilateralInfo(myGInfo);
        double[] gbuf = myBg.getBuffer();
        for (int i = 0; i < myGsize; i++) {
            gbuf[i] = -myGInfo[i].dist;
        }
        myRg.setZero();
        // mySys.getUnilateralOffsets (myRn, myBn, 0, MechSystem.POSITION_MODE);
        mySys.getUnilateralInfo(myNInfo);
        double[] nbuf = myBn.getBuffer();
        for (int i = 0; i < myNsize; i++) {
            nbuf[i] = -myNInfo[i].dist;
        }
        myRn.setZero();
        myVel.setZero();
        myLam.setZero();
        myThe.setZero();
        myRBSolver.updateStructure(myMass, myGT, myGTVersion);
        if (myRBSolver.projectPosition(myMass, myGT, myNT, myBg, myBn, myVel, myLam, myThe)) {
            mySys.addActivePosImpulse(pos, 1, myVel);
            return true;
        }
    }
    return false;
}

Example 2 with KKTSolver

use of maspack.solvers.KKTSolver in project artisynth_core by artisynth.

the class MechSystemSolver method computePosCorrections.

protected void computePosCorrections(VectorNd pos, VectorNd vel, double t) {
    boolean correctionNeeded = false;
    // assumes that updateMassMatrix() has been called
    int velSize = myActiveVelSize;
    if (velSize == 0) {
        return;
    }
    if (myConSolver == null) {
        myConSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    updateUnilateralConstraints();
    // myVel.setSize (velSize);
    if (myGsize > 0 || myNsize > 0) {
        boolean allConstraintsCompliant = true;
        mySys.getBilateralInfo(myGInfo);
        double[] gbuf = myBg.getBuffer();
        for (int i = 0; i < myGsize; i++) {
            ConstraintInfo gi = myGInfo[i];
            if (!myComplianceSupported || gi.compliance == 0) {
                gbuf[i] = -myGInfo[i].dist;
                allConstraintsCompliant = false;
            } else {
                gbuf[i] = 0;
            }
        }
        myRg.setZero();
        // mySys.getUnilateralOffsets (myRn, myBn, 0, MechSystem.POSITION_MODE);
        mySys.getUnilateralInfo(myNInfo);
        double[] nbuf = myBn.getBuffer();
        for (int i = 0; i < myNsize; i++) {
            ConstraintInfo ni = myNInfo[i];
            if (!myComplianceSupported || ni.compliance == 0) {
                nbuf[i] = -myNInfo[i].dist;
                allConstraintsCompliant = false;
            } else {
                nbuf[i] = 0;
            }
        }
        // only need to do the correction if some constraints are non-compliant
        if (!allConstraintsCompliant) {
            correctionNeeded = true;
            myRg.setZero();
            myRn.setZero();
            // System.out.println ("bn=" + myBn);
            myBf.setSize(velSize);
            myBf.setZero();
            if (myStabilization == PosStabilization.GlobalStiffness && integratorIsImplicit(myIntegrator)) {
                computeStiffnessPosCorrection(vel, velSize);
            } else {
                computeMassPosCorrection(vel, velSize);
            }
        }
    }
    if (correctionNeeded) {
        mySys.addActivePosImpulse(pos, 1, vel);
    }
}

Example 3 with KKTSolver

use of maspack.solvers.KKTSolver in project artisynth_core by artisynth.

the class MechSystemSolver method constrainedVelSolve.

protected void constrainedVelSolve(VectorNd vel, VectorNd f, double t0, double t1) {
    // assumes that updateMassMatrix() has been called
    double h = t1 - t0;
    int velSize = myActiveVelSize;
    if (velSize == 0) {
        return;
    }
    if (myConSolver == null) {
        myConSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    updateUnilateralConstraints();
    if (myGsize > 0 && myParametricVelSize > 0) {
        myGT.mulTranspose(myBg, myUpar, 0, myGsize, velSize, myParametricVelSize);
        // move to rhs
        myBg.negate();
    } else {
        myBg.setZero();
    }
    setBilateralOffsets(h, 0);
    if (myNsize > 0 && myParametricVelSize > 0) {
        myNT.mulTranspose(myBn, myUpar, 0, myNsize, velSize, myParametricVelSize);
        // move to rhs
        myBn.negate();
    // MatrixNd N = new MatrixNd();
    // N.set (myNT);
    // N.transpose ();
    // System.out.println ("N=\n" + N);
    // System.out.println ("myBn=" + myBn);
    } else {
        myBn.setZero();
    }
    setUnilateralOffsets(h, 0);
    myBf.setSize(velSize);
    if (myParametricVelSize > 0) {
        myMass.mul(myBf, myUpar, 0, velSize, velSize, myParametricVelSize);
        // move to rhs
        myBf.negate();
    } else {
        myBf.setZero();
    }
    myBf.scaledAdd(h, myMassForces);
    myBf.scaledAdd(h, f);
    // MatrixNd Mass = new MatrixNd (velSize, velSize);
    // myMass.getSubMatrix (0, 0, Mass);
    // MatrixNd GT = new MatrixNd (velSize, 6);
    // myGT.getSubMatrix (0, 0, GT);
    // GT.transpose ();
    // System.out.println ("GT size=" + myGT.getSize());
    // System.out.println ("Mass=\n" + Mass.toString ("%8.3f"));
    // System.out.println ("G=\n" + GT.toString ("%8.3f"));
    // System.out.println ("f=\n" + f.toString ("%8.3f"));
    myMass.mulAdd(myBf, vel, velSize, velSize);
    if (myConMassVersion != myMassVersion || myConGTVersion != myGTVersion) {
        myConSolver.analyze(myMass, velSize, myGT, myRg, Matrix.SPD);
        myConMassVersion = myMassVersion;
        myConGTVersion = myGTVersion;
    }
    // get these in case (at some future point) they are needed for warm
    // startin the solve
    mySys.getBilateralImpulses(myLam);
    mySys.getUnilateralImpulses(myThe);
    myConSolver.factor(myMass, velSize, myGT, myRg, myNT, myRn);
    myConSolver.solve(vel, myLam, myThe, myBf, myBg, myBn);
    if (computeKKTResidual) {
        double res = myConSolver.residual(myMass, velSize, myGT, myRg, myNT, myRn, vel, myLam, myThe, myBf, myBg, myBn);
        System.out.println("vel cor residual (" + velSize + "," + myGT.colSize() + "," + myNT.colSize() + "): " + res);
    }
    // vel.set (myVel);
    mySys.setBilateralImpulses(myLam, h);
    mySys.setUnilateralImpulses(myThe, h);
    if (myUpdateForcesAtStepEnd) {
        if (myGsize > 0) {
            myGT.mulAdd(myFcon, myLam, velSize, myGsize);
        }
        if (myNsize > 0) {
            myNT.mulAdd(myFcon, myThe, velSize, myNsize);
        }
    }
}

Example 4 with KKTSolver

use of maspack.solvers.KKTSolver in project artisynth_core by artisynth.

the class MechSystemSolver method computeStiffnessPosCorrection.

protected void computeStiffnessPosCorrection(VectorNd vel, int velSize) {
    boolean analyze = false;
    updateSolveMatrixStructure();
    if (myKKTSolveMatrixVersion != mySolveMatrixVersion) {
        myKKTSolveMatrixVersion = mySolveMatrixVersion;
        analyze = true;
    }
    SparseNumberedBlockMatrix S = mySolveMatrix;
    S.setZero();
    mySys.addVelJacobian(S, null, -1);
    mySys.addPosJacobian(S, null, -1);
    addActiveMassMatrix(mySys, S);
    if (myKKTSolver == null) {
        myKKTSolver = new KKTSolver();
        analyze = true;
    }
    if (myKKTGTVersion != myGTVersion) {
        analyze = true;
        myKKTGTVersion = myGTVersion;
    }
    if (analyze) {
        myKKTSolver.analyze(S, velSize, myGT, myRg, mySys.getSolveMatrixType());
    }
    if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
        myKKTSolver.factorAndSolve(S, velSize, myGT, myRg, vel, myLam, myBf, myBg, myHybridSolveTol);
    } else {
        myKKTSolver.factor(S, velSize, myGT, myRg, myNT, myRn);
        myKKTSolver.solve(vel, myLam, myThe, myBf, myBg, myBn);
    }
    if (computeKKTResidual) {
        double res = myKKTSolver.residual(S, velSize, myGT, myRg, myNT, myRn, vel, myLam, myThe, myBf, myBg, myBn);
        System.out.println("stiffness pos cor residual (" + velSize + "," + myGT.colSize() + "," + myNT.colSize() + "): " + res);
    }
}

Example 5 with KKTSolver

use of maspack.solvers.KKTSolver in project artisynth_core by artisynth.

the class MechSystemSolver method KKTStaticFactorAndSolve.

/**
 * Solves a static KKT system of the form
 * <pre>{@code
 * -df/dx*Delta(x) -G^T*lambda - N^T*theta = f
 * G*Delta(x) + g = 0, N*Delta(x) + n >= 0
 * }</pre>
 *
 * @param u returned displacement Delta(x)
 * @param bf right-hand side net force
 * @param beta scale factor for any additional forces such as fictitious forces
 * @param btmp temporary vector
 */
public void KKTStaticFactorAndSolve(VectorNd u, VectorNd bf, double beta, VectorNd btmp) {
    updateStateSizes();
    int velSize = myActiveVelSize;
    boolean analyze = false;
    updateSolveMatrixStructure();
    if (myStaticKKTVersion != mySolveMatrixVersion) {
        myStaticKKTVersion = mySolveMatrixVersion;
        analyze = true;
    }
    SparseNumberedBlockMatrix S = mySolveMatrix;
    S.setZero();
    // add tikhonov regularization factor
    if (myStaticTikhonov > 0) {
        // identity
        for (int i = 0; i < S.numBlockRows(); ++i) {
            MatrixBlock bi = S.getBlock(i, i);
            for (int j = 0; j < bi.rowSize(); ++j) {
                bi.set(j, j, myStaticTikhonov);
            }
        }
    }
    myC.setZero();
    mySys.addPosJacobian(S, myC, -1);
    if (useFictitousJacobianForces && beta != 0) {
        bf.scaledAdd(beta, myC);
    }
    if (myStaticSolver == null) {
        myStaticSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    if (myKKTGTVersion != myGTVersion) {
        analyze = true;
        myKKTGTVersion = myGTVersion;
    }
    // bilateral offsets
    // setBilateralOffsets (h, -a0); // -a0);
    // myVel.setSize (velSize);
    getBilateralDeviation(myBg);
    myRg.setZero();
    updateUnilateralConstraints();
    getUnilateralDeviation(myBn);
    // if (myStaticTikhonov < 0) {
    // double fn2 = S.frobeniusNormSquared();
    // if (myGsize > 0) {
    // fn2 += myGT.frobeniusNormSquared();
    // }
    // if (myNsize > 0) {
    // fn2 += myNT.frobeniusNormSquared();
    // }
    // double eps = Math.sqrt(0.1*Math.sqrt(fn2/velSize));
    // // add scaled identity
    // for (int i=0; i<S.numBlockRows(); ++i) {
    // MatrixBlock bi = S.getBlock(i, i);
    // for (int j=0; j<bi.rowSize(); ++j) {
    // bi.set(j,j, bi.get(j, j)+eps);
    // }
    // }
    // System.out.println("Tikhonov: " + eps);
    // }
    // get these in case we are doing hybrid solves and they are needed to
    // help with a warm start
    mySys.getBilateralImpulses(myLam);
    mySys.getUnilateralImpulses(myThe);
    if (!solveModePrinted) {
        String msg = (myHybridSolveP ? "hybrid solves" : "direct solves");
        if (mySys.getSolveMatrixType() == Matrix.INDEFINITE) {
            msg += ", unsymmetric matrix";
        } else {
            msg += ", symmetric matrix";
        }
        System.out.println(msg);
        solveModePrinted = true;
    }
    if (crsWriter == null && crsFileName != null) {
        try {
            crsWriter = ArtisynthIO.newIndentingPrintWriter(crsFileName);
        } catch (Exception e) {
            crsFileName = null;
        }
    }
    if (velSize != 0) {
        u.setZero();
        if (analyze) {
            myStaticSolver.analyze(S, velSize, myGT, myRg, mySys.getSolveMatrixType());
        }
        if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myStaticSolver.factorAndSolve(S, velSize, myGT, myRg, u, myLam, bf, myBg, myHybridSolveTol);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve(hybrid)");
            }
        } else {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myStaticSolver.factor(S, velSize, myGT, myRg, myNT, myRn);
            // int nperturbed = myStaticSolver.getNumNonZerosInFactors();
            myStaticSolver.solve(u, myLam, myThe, bf, myBg, myBn);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve");
            }
        }
        if (computeKKTResidual) {
            double res = myStaticSolver.residual(S, velSize, myGT, myRg, myNT, myRn, u, myLam, myThe, bf, myBg, myBn);
            System.out.println("vel residual (" + velSize + "," + myGT.colSize() + "," + myNT.colSize() + "): " + res);
        }
        if (crsWriter != null) {
            String msg = "# KKTsolve M=" + velSize + " G=" + myGT.colSize() + " N=" + myNT.colSize() + (analyze ? " ANALYZE" : "");
            System.out.println(msg);
            try {
                crsWriter.println(msg);
                myStaticSolver.printLinearProblem(crsWriter, bf, myBg, "%g", crsOmitDiag);
            } catch (Exception e) {
                e.printStackTrace();
                crsWriter = null;
                crsFileName = null;
            }
        }
    }
    mySys.setBilateralImpulses(myLam, 1);
    mySys.setUnilateralImpulses(myThe, 1);
    if (myUpdateForcesAtStepEnd) {
        if (myGsize > 0) {
            myGT.mulAdd(myFcon, myLam, velSize, myGsize);
        }
        if (myNsize > 0) {
            myNT.mulAdd(myFcon, myThe, velSize, myNsize);
        }
    }
    if (myLogWriter != null) {
        try {
            NumberFormat fmt = new NumberFormat("%g");
            myLogWriter.println("M(" + velSize + "x" + velSize + ")=[");
            S.write(myLogWriter, fmt, Matrix.WriteFormat.Dense, velSize, velSize);
            myLogWriter.println("];");
            myLogWriter.println("GT(" + velSize + "x" + myGT.colSize() + ")=[");
            myGT.write(myLogWriter, fmt, Matrix.WriteFormat.Dense, velSize, myGT.colSize());
            myLogWriter.println("];");
            myLogWriter.println("NT(" + velSize + "x" + myNT.colSize() + ")=[");
            myNT.write(myLogWriter, fmt, Matrix.WriteFormat.Dense, velSize, myNT.colSize());
            myLogWriter.println("];");
            myLogWriter.println("bf=[");
            bf.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myBg=[");
            myBg.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myBn=[");
            myBn.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("u=[");
            u.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myLam=[");
            myLam.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("myThe=[");
            myThe.write(myLogWriter, fmt);
            myLogWriter.println("];");
            myLogWriter.println("");
            myLogWriter.flush();
            System.out.println("logging");
        } catch (IOException e) {
            e.printStackTrace();
            myLogWriter = null;
        }
    }
}