Examples with SparseBlockMatrix maspack.matrix.SparseBlockMatrix used on opensource projects

Example 6 with SparseBlockMatrix

use of maspack.matrix.SparseBlockMatrix in project artisynth_core by artisynth.

the class MechSystemBase method getAttachmentConstraints.

/**
 * Returns the transpose of the attachment constraint matrix.  This is used
 * only for debugging.
 *
 * @return transposed attachment constraint matrix
 */
public SparseBlockMatrix getAttachmentConstraints() {
    updateDynamicComponentLists();
    // figure out the column sizes
    ArrayList<DynamicAttachment> attachments = getOrderedAttachments();
    int[] colSizes = new int[attachments.size()];
    for (int k = 0; k < attachments.size(); k++) {
        DynamicComponent slave = attachments.get(k).getSlave();
        int bj = attachments.size() - 1 - k;
        colSizes[bj] = myDynamicSizes[slave.getSolveIndex()];
    }
    SparseBlockMatrix GT = new SparseBlockMatrix(myDynamicSizes, colSizes);
    for (int k = 0; k < attachments.size(); k++) {
        int bj = attachments.size() - 1 - k;
        DynamicAttachment a = attachments.get(k);
        int ssize = colSizes[bj];
        DynamicComponent[] masters = a.getMasters();
        DynamicComponent slave = a.getSlave();
        MatrixNdBlock sblk = new MatrixNdBlock(ssize, ssize);
        sblk.setIdentity();
        GT.addBlock(slave.getSolveIndex(), bj, sblk);
        for (int i = 0; i < masters.length; i++) {
            int bi = masters[i].getSolveIndex();
            int msize = myDynamicSizes[bi];
            MatrixNdBlock mblk = new MatrixNdBlock(msize, ssize);
            a.mulSubGT(mblk, sblk, i);
            mblk.negate();
            GT.addBlock(bi, bj, mblk);
        }
    }
    return GT;
}

Example 7 with SparseBlockMatrix

use of maspack.matrix.SparseBlockMatrix in project artisynth_core by artisynth.

the class TrackingController method apply.

/**
 * Applies the controller, estimating and setting the next
 * set of muscle activations
 */
public void apply(double t0, double t1) {
    // System.out.println("dt = "+(t1-t0)+"     h = "+ TimeBase.round(t1 - t0));
    if (getDebug()) {
        // cleans up the console
        System.out.println("\n--- t = " + t1 + " ---");
    }
    if (t0 == 0) {
        // XXX need better way to zero excitations on reset
        myCostFunction.setSize(numExcitations());
        myExcitations = new VectorNd(numExcitations());
    }
    if (!isEnabled()) {
        return;
    }
    // need to save forces so that we can restore them at the end
    VectorNd savedForces = new VectorNd();
    myMech.getForces(savedForces);
    prevExcitations.set(myExcitations);
    SparseBlockMatrix Jc = (myForceTerm == null ? null : myForceTerm.getForceJacobian());
    SparseBlockMatrix Jm = (myMotionTerm == null ? null : myMotionTerm.getVelocityJacobian());
    // update and store inverse data
    myMotionForceData.update(t0, t1, Jm, Jc);
    if (myMotionTerm.useDeltaAct) {
        VectorNd deltaActivations = myCostFunction.solve(t0, t1);
        myExcitations.add(deltaActivations);
        if (getDebug()) {
            System.out.println("da = [" + deltaActivations.toString("%.4f") + "];");
        }
    } else {
        myExcitations.set(myCostFunction.solve(t0, t1));
    }
    /*
       * limit excitation jumps
       */
    // System.out.println ("excitations="+myExcitations);
    NumberFormat f4 = new NumberFormat("%.4f");
    /* debug info */
    if (getDebug()) {
    // System.out.println("ex = ["+myExcitations.toString (f4)+"];");
    // System.out.println("lb = ["+lowerBound.toString (f4)+"];");
    // System.out.println("ub = ["+upperBound.toString (f4)+"];");
    }
    setExcitations(myExcitations, 0);
    myMech.setForces(savedForces);
// if (kTerm != null) {
// System.out.println("K* = "+kTerm.getStiffnessTargetVec().toString("%8.2f"));
// System.out.println("K  = "+kTerm.getStiffnessVec().toString("%8.2f")+"\n");
// }
// if (cTerm != null) {
// System.out.println("C* = "+cTerm.getComplianceTargetVec().toString("%8.6f"));
// System.out.println("C  = "+cTerm.getComplianceVec().toString("%8.6f")+"\n");
// }
}