Examples with Wrench maspack.spatialmotion.Wrench used on opensource projects

Example 1 with Wrench

use of maspack.spatialmotion.Wrench in project artisynth_core by artisynth.

the class LemkeContactSolver method initialize.

private void initialize(ContactInfo[] contacts, int ncontacts, SpatialInertia inertia, Wrench wapplied, int ndirs) {
    Point3d pnt = new Point3d();
    setSize(ncontacts, ndirs);
    myInertia = inertia;
    // vdroot = Math.sqrt(vdamping);
    // wdroot = Math.sqrt(wdamping);
    contactInfo = contacts;
    if (myInertia != null) {
        myInertia.mulLeftFactorInverse(wapplyAdjusted, wapplied);
    }
    // wapplyAdjusted.f.scale (1/vdroot, wapplied.f);
    // wapplyAdjusted.m.scale (1/wdroot, wapplied.m);
    // System.out.println ("wapp=" + wapplyAdjusted.toString("%8.3f"));
    wtotalAdjusted.set(wapplyAdjusted);
    // initialize points, contact normals, and first variables
    for (int i = 0; i < numc; i++) {
        Wrench N = NDBuffer[i];
        N.f.set(contactInfo[i].nrm);
        N.m.cross(contactInfo[i].pnt, contactInfo[i].nrm);
        bq[i] = contactInfo[i].normalVelocityLimit;
        if (myInertia != null) {
            myInertia.mulLeftFactorInverse(N, N);
        }
        zVars[i].init(THETA, i, -1, wVars[i], N);
        wVars[i].init(NU, i, -1, zVars[i], N);
        frictionStatus[i] = FRICTION_INACTIVE;
        // qv[i] = N.dot(wapplyAdjusted);
        c[i] = 1;
        varToActive[i] = i;
        activeToVar[i] = i;
    }
    zVars[numc].init(Z0, -1, -1, null, null);
    z0Var = zVars[numc];
}

Example 2 with Wrench

use of maspack.spatialmotion.Wrench in project artisynth_core by artisynth.

the class LemkeContactSolver method addRestitution.

private void addRestitution(Twist vel, double restitution) {
    // compute the velocity impulse that was applied along all the
    // normals
    Wrench fnorm = new Wrench();
    Twist vnorm = new Twist();
    int kz = 0;
    for (ContactVariable the = thetaAlpha.head; the != null; the = the.next) {
        double value = zk.get(kz++);
        fnorm.scaledAdd(value, the.nd, fnorm);
    }
    vnorm.set(fnorm);
    if (myInertia != null) {
        myInertia.mulRightFactorInverse(vnorm, vnorm);
    }
    vel.scaledAdd(restitution, vnorm, vel);
}

Example 3 with Wrench

use of maspack.spatialmotion.Wrench in project artisynth_core by artisynth.

the class DantzigLCPSolverTest method testMultiPointContact.

/**
 * Create a test case involving multi-point contact of a box on a plane. The
 * angle of the plane normal relative to the horizontal is ang, and the
 * friction coefficient is mu.
 */
public void testMultiPointContact(double ang, double mu) {
    double mass = 4.0;
    SpatialInertia Inertia = SpatialInertia.createBoxInertia(mass, 2.0, 1.0, 2.0);
    Point3d[] pnts = new Point3d[] { new Point3d(1.0, -0.5, 1.0), new Point3d(1.0, -0.5, -1.0), new Point3d(-1.0, -0.5, -1.0) // new Point3d (-1.0, -0.5, 1.0),
    };
    int nump = pnts.length;
    int numc = 3 * pnts.length;
    Wrench[] constraints = new Wrench[numc];
    for (int i = 0; i < pnts.length; i++) {
        Wrench NT = new Wrench();
        NT.f.set(Vector3d.Y_UNIT);
        NT.m.cross(pnts[i], NT.f);
        constraints[i] = NT;
        Wrench DT = new Wrench();
        DT.f.set(Vector3d.X_UNIT);
        DT.m.cross(pnts[i], DT.f);
        constraints[pnts.length + 2 * i] = DT;
        DT = new Wrench();
        DT.f.set(Vector3d.Z_UNIT);
        DT.m.cross(pnts[i], DT.f);
        constraints[pnts.length + 2 * i + 1] = DT;
    }
    Twist vel0 = new Twist(Math.sin(ang), -Math.cos(ang), 0, 0, 0, 0);
    vel0.scale(9.8);
    MatrixNd A = new MatrixNd(nump, nump);
    VectorNd b = new VectorNd(nump);
    VectorNd theEst = new VectorNd(nump);
    Twist tw = new Twist();
    Wrench wr = new Wrench();
    for (int i = 0; i < nump; i++) {
        for (int j = 0; j < nump; j++) {
            Inertia.mulInverse(tw, constraints[j]);
            A.set(i, j, constraints[i].dot(tw));
        }
        b.set(i, -constraints[i].dot(vel0));
    // System.out.println ("D=" + constraints[i].toString ("%8.3f"));
    }
    CholeskyDecomposition Chol = new CholeskyDecomposition();
    Chol.factor(A);
    Chol.solve(theEst, b);
    // System.out.println ("vel0=" + vel0.toString ("%8.3f"));
    // System.out.println ("theEst=" + theEst.toString ("%8.3f"));
    // for (int i=0; i<nump; i++)
    // { wr.scale (theEst.get(i), constraints[i]);
    // Inertia.mulInverse (tw, wr);
    // vel0.add (tw);
    // }
    // System.out.println ("vel0=" + vel0.toString("%8.3f"));
    MatrixNd M = new MatrixNd(numc, numc);
    VectorNd q = new VectorNd(numc);
    VectorNd z = new VectorNd(numc);
    VectorNd w = new VectorNd(numc);
    VectorNd lo = new VectorNd(numc);
    VectorNd hi = new VectorNd(numc);
    for (int i = 0; i < numc; i++) {
        for (int j = 0; j < numc; j++) {
            Inertia.mulInverse(tw, constraints[j]);
            M.set(i, j, constraints[i].dot(tw));
        }
        q.set(i, constraints[i].dot(vel0));
    }
    for (int i = 0; i < pnts.length; i++) {
        lo.set(i, 0);
        hi.set(i, inf);
        lo.set(pnts.length + 2 * i, -theEst.get(i) * mu);
        hi.set(pnts.length + 2 * i, theEst.get(i) * mu);
        lo.set(pnts.length + 2 * i + 1, -theEst.get(i) * mu);
        hi.set(pnts.length + 2 * i + 1, theEst.get(i) * mu);
    }
    // System.out.println ("ang=" + Math.toDegrees (ang));
    testSolver(z, w, M, q, lo, hi, 0, numc, DantzigLCPSolver.Status.SOLVED);
    Twist vel = new Twist(vel0);
    for (int i = 0; i < numc; i++) {
        wr.scale(z.get(i), constraints[i]);
        Inertia.mulInverse(tw, wr);
        vel.add(tw);
    }
    // }
    if (ang <= Math.atan(mu)) {
        if (!vel.epsilonEquals(Twist.ZERO, 1e-8)) {
            throw new TestException("velocity should be 0 with ang=" + Math.toDegrees(ang));
        }
    } else {
        if (vel.epsilonEquals(Twist.ZERO, 1e-8)) {
            throw new TestException("velocity should be non-zero with ang=" + Math.toDegrees(ang));
        }
    // if (Math.abs(Math.abs(z.get(1)) - theEst*mu) > 1e-8)
    // { throw new TestException (
    // "friction force not on the cone");
    // }
    }
}

Example 4 with Wrench

use of maspack.spatialmotion.Wrench in project artisynth_core by artisynth.

the class FemModel3d method mulInverseMass.

@Override
public void mulInverseMass(SparseBlockMatrix M, VectorNd a, VectorNd f) {
    double[] abuf = a.getBuffer();
    double[] fbuf = f.getBuffer();
    int asize = a.size();
    if (M.getAlignedBlockRow(asize) == -1) {
        throw new IllegalArgumentException("size of 'a' not block aligned with 'M'");
    }
    if (f.size() < asize) {
        throw new IllegalArgumentException("size of 'f' is less than the size of 'a'");
    }
    int bf = myFrame.getSolveIndex();
    int bk;
    if (myFrameRelativeP && bf < asize) {
        int fidx = M.getBlockRowOffset(bf);
        Wrench wtmp = new Wrench();
        double[] tmp6 = new double[6];
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    MatrixBlock Mfk = M.getBlock(bf, bk);
                    zero6Vector(tmp6);
                    Mfk.mulAdd(tmp6, 0, fbuf, idx);
                    Matrix3x3Block Mkk = (Matrix3x3Block) M.getBlock(bk, bk);
                    // XXX do we want to use n.mulInverseEffectiveMass?
                    scaledAdd(wtmp, -1 / Mkk.m00, tmp6, 0);
                }
            }
        }
        scaledAdd(wtmp, 1, fbuf, fidx);
        // XXX do we need to use getBlock (bf, bf)???
        Matrix6dBlock Mff = (Matrix6dBlock) M.getBlock(bf, bf);
        SpatialInertia S = new SpatialInertia();
        S.set(Mff);
        Twist ttmp = new Twist();
        S.mulInverse(ttmp, wtmp);
        setFromTwist(abuf, fidx, ttmp);
        double[] tmp3 = new double[3];
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    MatrixBlock Mkf = M.getBlock(bk, bf);
                    Matrix3x3Block Mkk = (Matrix3x3Block) M.getBlock(bk, bk);
                    double minv = 1 / Mkk.m00;
                    zero3Vector(tmp3);
                    Mkf.mulAdd(tmp3, 0, abuf, fidx);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[0]);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[1]);
                    abuf[idx++] = minv * (fbuf[idx] - tmp3[2]);
                }
            }
        }
    } else {
        for (int i = 0; i < myNodes.size(); i++) {
            FemNode3d n = myNodes.get(i);
            if ((bk = n.getSolveIndex()) != -1) {
                int idx = M.getBlockRowOffset(bk);
                if (idx < asize) {
                    n.mulInverseEffectiveMass(M.getBlock(bk, bk), abuf, fbuf, idx);
                }
            }
        }
    }
}

Example 5 with Wrench

use of maspack.spatialmotion.Wrench in project artisynth_core by artisynth.

the class FrameSpring method copy.

@Override
public ModelComponent copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    FrameSpring comp = (FrameSpring) super.copy(flags, copyMap);
    if (myRenderProps != null) {
        comp.setRenderProps(myRenderProps);
    }
    if (myX1A != RigidTransform3d.IDENTITY) {
        comp.myX1A = new RigidTransform3d(myX1A);
    }
    if (myX2B != RigidTransform3d.IDENTITY) {
        comp.myX2B = new RigidTransform3d(myX2B);
    }
    comp.myF = new Wrench();
    comp.myFTmp = new Wrench();
    comp.myVel1 = new Twist();
    comp.myVel2 = new Twist();
    comp.myVel21 = new Twist();
    comp.myTmp00 = new Matrix6d();
    comp.myTmp01 = new Matrix6d();
    comp.myTmp10 = new Matrix6d();
    comp.myTmp11 = new Matrix6d();
    comp.myRenderFrame = new RigidTransform3d();
    comp.myRenderPnt1 = new float[3];
    comp.myRenderPnt2 = new float[3];
    comp.myTmpM = new Matrix3d();
    // comp.myRBA = new RotationMatrix3d();
    comp.myX21 = new RigidTransform3d();
    comp.myMaterial = (FrameMaterial) myMaterial.clone();
    return comp;
}