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

Example 1 with SpatialInertia

use of maspack.spatialmotion.SpatialInertia 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 2 with SpatialInertia

use of maspack.spatialmotion.SpatialInertia 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 3 with SpatialInertia

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

the class RigidBody method setInertiaFromMesh.

private void setInertiaFromMesh(double density) {
    PolygonalMesh mesh = null;
    if ((mesh = getMesh()) == null) {
        throw new IllegalStateException("Mesh has not been set");
    }
    SpatialInertia M = mesh.createInertia(density);
    mySpatialInertia.set(M);
}

Example 4 with SpatialInertia

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

the class RigidBody method setInertia.

/**
 * Explicitly sets the mass, rotational inertia, and center of mass of this
 * body. Also sets the uniform density (as returned by {@link #getDensity
 * getDensity}) to be undefined).
 */
public void setInertia(double m, SymmetricMatrix3d J, Point3d com) {
    SpatialInertia M = new SpatialInertia();
    M.set(m, J, com);
    doSetInertia(M);
}

Example 5 with SpatialInertia

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

the class RigidCompositeBody method setInertia.

/**
 * Explicitly sets the mass and rotational inertia of this body. Also sets
 * the uniform density (as returned by {@link #getDensity getDensity}) to be
 * undefined).
 */
public void setInertia(double m, SymmetricMatrix3d J) {
    SpatialInertia M = new SpatialInertia();
    M.set(m, J);
    doSetInertia(M);
}