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

Example 6 with KKTSolver

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

the class RigidBodySolver method updateStructure.

public void updateStructure(SparseBlockMatrix M, SparseBlockMatrix GT, int GTversion) {
    if (mySolver == null) {
        mySolver = new KKTSolver();
    }
    if (myStructureVersion != mySys.getStructureVersion() || myBilateralVersion != GTversion) {
        doUpdateStructure(M, GT);
        myStructureVersion = mySys.getStructureVersion();
        myBilateralVersion = GTversion;
    }
}

Example 7 with KKTSolver

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

the class MechSystemSolver method KKTFactorAndSolve.

/**
 * Solves a KKT system in which the Jacobian augmented M matrix and
 * and force vectors are given by
 * <pre>
 * M' = M + a0 df/dv + a1 df/dx
 *
 * bf' = bf + (a2 df/dv + a3 df/dx) vel0
 * </pre>
 * It is assumed that a0 and a1 are both non-zero. It is also assumed that
 * the a0 = -alpha h, where h is the step size and alpha indicates the
 * propertion of implicitness for the solve; i.e., for regular backward
 * euler, alpha=1, while for trapezoidal solves, alpha = 0.5;
 *
 * When used to solve for velocities in an implicit integrator, then
 * on input, bf is assumed to be given by
 * <pre>
 * bf = M vel0 + h f
 * </pre>
 * where h is the time step and f is the generalized forces, while
 * on output bf is modified to include the Jacobian terms described
 * above.
 *
 * @param vel returns the computed velocity
 * @param fpar if useFictitousJacobianForces is true, returns fictitious
 * Jacobian forces for parametric components
 * @param bf right side offset
 * @param btmp temporary vector
 * @param vel0 right side velocity
 * @param h interval time step - used to scale constraint offsets and
 * impulses
 * @param a0 left side df/dv coefficient
 * @param a1 left side df/dx coefficient
 * @param a2 right side df/dv coefficient
 * @param a3 right side df/dx coefficient
 */
public void KKTFactorAndSolve(VectorNd vel, VectorNd fpar, VectorNd bf, VectorNd btmp, VectorNd vel0, double h, double a0, double a1, double a2, double a3) {
    // assumes that updateMassMatrix() has been called
    updateStateSizes();
    int velSize = myActiveVelSize;
    boolean analyze = false;
    updateSolveMatrixStructure();
    if (myKKTSolveMatrixVersion != mySolveMatrixVersion) {
        myKKTSolveMatrixVersion = mySolveMatrixVersion;
        analyze = true;
    }
    SparseNumberedBlockMatrix S = mySolveMatrix;
    S.setZero();
    myC.setSize(S.rowSize());
    myC.setZero();
    mySys.addVelJacobian(S, myC, a0);
    // System.out.println ("myC=" + myC);
    if (useFictitousJacobianForces) {
        bf.scaledAdd(-a0, myC);
        if (fpar != null && myParametricVelSize > 0) {
            setSubVector(fpar, myC, velSize, myParametricVelSize);
        }
    }
    if (vel0 != null) {
        double alpha = a2 / a0 - a3 / a1;
        S.mul(btmp, vel0, velSize, velSize);
        bf.scaledAdd(alpha, btmp);
    }
    myC.setZero();
    mySys.addPosJacobian(S, myC, a1);
    if (useFictitousJacobianForces) {
        bf.scaledAdd(-a0, myC);
        if (fpar != null && myParametricVelSize > 0) {
            addSubVector(fpar, myC, velSize, myParametricVelSize);
        }
    }
    if (vel0 != null && a3 != 0) {
        double beta = a3 / a1;
        S.mul(btmp, vel0, velSize, velSize);
        bf.scaledAdd(beta, btmp);
    }
    addActiveMassMatrix(mySys, S);
    if (velSize > 0 && myParametricVelSize > 0) {
        S.mulTranspose(btmp, myUpar, 0, velSize, velSize, myParametricVelSize);
        bf.sub(btmp);
    }
    if (myKKTSolver == null) {
        myKKTSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    if (myKKTGTVersion != myGTVersion) {
        analyze = true;
        myKKTGTVersion = myGTVersion;
    }
    if (myGsize > 0 && myParametricVelSize > 0) {
        myGT.mulTranspose(myBg, myUpar, 0, myGsize, velSize, myParametricVelSize);
        // move to rhs
        myBg.negate();
    } else {
        myBg.setZero();
    }
    // a0 is assumed to be negative, which moves myGdot over to the rhs
    // myBg.scaledAdd (a0, myGdot);
    // -a0);
    setBilateralOffsets(h, -a0);
    updateUnilateralConstraints();
    if (myNsize > 0 && myParametricVelSize > 0) {
        myNT.mulTranspose(myBn, myUpar, 0, myNsize, velSize, myParametricVelSize);
        // move to rhs
        myBn.negate();
    } else {
        myBn.setZero();
    }
    // a0 is assumed to be negative, which moves myNdot over to the rhs
    // -a0);
    setUnilateralOffsets(h, -a0);
    // 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) {
        if (vel0 != null) {
            // set vel to vel0 in case the solver needs a warm start
            vel.set(vel0);
        }
        if (analyze) {
            myKKTSolver.analyze(S, velSize, myGT, myRg, mySys.getSolveMatrixType());
        }
        if (myHybridSolveP && !analyze && myNT.colSize() == 0) {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myKKTSolver.factorAndSolve(S, velSize, myGT, myRg, vel, myLam, bf, myBg, myHybridSolveTol);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve(hybrid)");
            }
        } else {
            if (profileKKTSolveTime) {
                timerStart();
            }
            myKKTSolver.factor(S, velSize, myGT, myRg, myNT, myRn);
            myKKTSolver.solve(vel, myLam, myThe, bf, myBg, myBn);
            if (profileKKTSolveTime) {
                timerStop("KKTsolve");
            }
        }
        if (computeKKTResidual) {
            double res = myKKTSolver.residual(S, velSize, myGT, myRg, myNT, myRn, vel, 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);
                myKKTSolver.printLinearProblem(crsWriter, bf, myBg, "%g", crsOmitDiag);
            } catch (Exception e) {
                e.printStackTrace();
                crsWriter = null;
                crsFileName = null;
            }
        }
    }
    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);
        }
    }
    if (myLogWriter != null) {
        try {
            NumberFormat fmt = new NumberFormat("%g");
            myLogWriter.println("M(" + velSize + "x" + velSize + ")=[");
            S.write(myLogWriter, fmt, Matrix.WriteFormat.SYMMETRIC_CRS, velSize, velSize);
            myLogWriter.println("];");
            myLogWriter.println("GT(" + velSize + "x" + myGT.colSize() + ")=[");
            myGT.write(myLogWriter, fmt, Matrix.WriteFormat.CRS, velSize, myGT.colSize());
            myLogWriter.println("];");
            myLogWriter.println("NT(" + velSize + "x" + myNT.colSize() + ")=[");
            myNT.write(myLogWriter, fmt, Matrix.WriteFormat.CRS, 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("vel=[");
            vel.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;
        }
    }
}

Example 8 with KKTSolver

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

the class MechSystemSolver method computeVelCorrections.

protected void computeVelCorrections(VectorNd vel, double t0, double t1) {
    double h = t1 - t0;
    // assumes that updateMassMatrix() has been called
    int velSize = myActiveVelSize;
    if (velSize == 0) {
        return;
    }
    if (myConSolver == null) {
        myConSolver = new KKTSolver();
    }
    updateBilateralConstraints();
    updateUnilateralConstraints();
    if (myGsize == 0 && myNsize == 0) {
        // no constraints, so no solve needed
        return;
    }
    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);
    // mySys.getBilateralOffsets (myRg, myBg, 0, MechSystem.VELOCITY_MODE);
    // mySys.getUnilateralOffsets (myRn, myBn, 0, MechSystem.VELOCITY_MODE);
    myBf.setSize(velSize);
    // myVel.setSize (velSize);
    // TODO: need to add fictitous forces
    myMass.mul(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);
        }
    }
}