Search in sources :

Example 6 with Wrench

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

the class Frame method copy.

public Frame copy(int flags, Map<ModelComponent, ModelComponent> copyMap) {
    Frame comp = (Frame) super.copy(flags, copyMap);
    comp.myState = new FrameState();
    comp.myForce = new Wrench();
    comp.myExternalForce = new Wrench();
    comp.myRenderFrame = new RigidTransform3d();
    comp.myAxisLength = myAxisLength;
    comp.mySolveBlock = null;
    comp.mySolveBlockNum = -1;
    comp.mySolveBlockValidP = false;
    comp.setPose(getPose());
    return comp;
}
Also used : HasCoordinateFrame(artisynth.core.modelbase.HasCoordinateFrame) Wrench(maspack.spatialmotion.Wrench) RigidTransform3d(maspack.matrix.RigidTransform3d)

Example 7 with Wrench

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

the class RigidBody method mulInverseEffectiveMass.

public static int mulInverseEffectiveMass(SpatialInertia S, double[] a, double[] f, int idx) {
    Twist tw = new Twist();
    Wrench wr = new Wrench();
    wr.f.x = f[idx];
    wr.f.y = f[idx + 1];
    wr.f.z = f[idx + 2];
    wr.m.x = f[idx + 3];
    wr.m.y = f[idx + 4];
    wr.m.z = f[idx + 5];
    S.mulInverse(tw, wr);
    a[idx++] = tw.v.x;
    a[idx++] = tw.v.y;
    a[idx++] = tw.v.z;
    a[idx++] = tw.w.x;
    a[idx++] = tw.w.y;
    a[idx++] = tw.w.z;
    return idx;
}
Also used : Twist(maspack.spatialmotion.Twist) Wrench(maspack.spatialmotion.Wrench)

Example 8 with Wrench

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

the class LemkeContactSolver method activateFriction.

private void activateFriction(int ci) {
    // boolean z0isBasic = z0Var.isBasic;
    // System.out.println ("activating friction " + ci);
    zVars[numVars + numd + 1].set(z0Var);
    z0Var = zVars[numVars + numd + 1];
    int di = numVars;
    if (frictionStatus[ci] == FRICTION_INACTIVE) {
        // first time friction
        // activated, so compute
        // friction directions
        Point3d pnt = contactInfo[ci].pnt;
        // set x direction to be parallel to current tangent velocity
        // current body
        computeVelocity(currentVel, wtotalAdjusted);
        // velocity
        // compute tangent velocity
        vtmp.cross(currentVel.w, pnt);
        vtmp.add(currentVel.v);
        ydir.cross(vtmp, contactInfo[ci].nrm);
        double mag = ydir.norm();
        if (smartDirections && mag >= WORKING_PREC) {
            ydir.scale(1 / mag);
            xdir.cross(ydir, contactInfo[ci].nrm);
            xdir.normalize();
        }
        // else
        {
            R.setIdentity();
            R.setZDirection(contactInfo[ci].nrm);
            R.getColumn(0, xdir);
            R.getColumn(1, ydir);
        }
        for (int j = 0; j < numd; j++) {
            double cos = Math.cos(2 * Math.PI * j / (double) numd);
            double sin = Math.sin(2 * Math.PI * j / (double) numd);
            Wrench D = NDBuffer[numc + ci * numd + j];
            D.f.combine(cos, xdir, sin, ydir);
            D.m.cross(pnt, D.f);
            bq[di + j] = D.dot(contactInfo[ci].otherBodyVelocity);
            if (myInertia != null) {
                myInertia.mulLeftFactorInverse(D, D);
            }
        }
    }
    for (int j = 0; j < numd; j++) {
        Wrench D = NDBuffer[numc + ci * numd + j];
        int idx = di + j;
        zVars[idx].init(PHI, ci, di, wVars[idx], D);
        wVars[idx].init(SIGMA, ci, di, zVars[idx], D);
        c[idx] = 1;
    }
    int idx = di + numd;
    zVars[idx].init(LAMBDA, ci, di, wVars[idx], null);
    wVars[idx].init(GAMMA, ci, di, zVars[idx], null);
    c[idx] = 1;
    bq[idx] = 0;
    zVars[ci].di = di;
    wVars[ci].di = di;
    for (int j = 0; j < numd + 1; j++) {
        varToActive[numVars + j] = numActiveVars + j;
        activeToVar[numActiveVars + j] = numVars + j;
    }
    frictionStatus[ci] = FRICTION_ACTIVE;
    numVars += (numd + 1);
    numActiveVars += (numd + 1);
}
Also used : Wrench(maspack.spatialmotion.Wrench) Point3d(maspack.matrix.Point3d)

Example 9 with Wrench

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

the class LemkeContactSolver method solve.

// private void printStepInfo (
// String msg, double[] mv, double[] qv, ContactVariable driveVar,
// ContactVariable blockingVar, int[] candidates, int numCand)
// {
// int[] indices = new int[numActiveVars];
// Variable[] vars = new Variable[numActiveVars];
// // int dec = 0;
// // for (int i=0; i<numVars; i++)
// // { if (wVars[i].di == i && !frictionActive(wVars[i]))
// // { dec += numd+1;
// // i += numd;
// // }
// // else if (wVars[i].isBasic)
// // { vars[i-dec] = wVars[i];
// // }
// // else if (zVars[i].isBasic)
// // { vars[i-dec] = zVars[i];
// // }
// // else
// // { vars[i-dec] = z0Var;
// // }
// // }
// // System.out.println ("dec=" + dec);
// for (int i=0; i<numActiveVars; i++)
// { int vari = activeToVar[i];
// if (wVars[vari].isBasic)
// { vars[i] = wVars[vari];
// }
// else if (zVars[vari].isBasic)
// { vars[i] = zVars[vari];
// }
// else
// { vars[i] = z0Var;
// }
// }
// int i = 0;
// // ContactVariable[] zAlpha = getZAlphaVars();
// // int i = 0;
// // for (int k=0; k<zAlpha.length; k++)
// // { indices[i++] = zAlpha[k] == z0Var ? wzVar.idx : zAlpha[k].idx;
// // }
// // for (ContactVariable wvar=wBeta.head; wvar!=null; wvar=wvar.next)
// // { indices[i++] = wvar.idx;
// // }
// for (i=0; i<numActiveVars; i++)
// { indices[i] = i;
// }
// // if (numVars != numActiveVars)
// // { for (i=0; i<numActiveVars; i++)
// // { indices[i] = compressIndex (indices[i]);
// // System.out.println (indices[i]);
// // }
// // }
// printStepInfo (msg, vars, mv, qv, numActiveVars, driveVar,
// blockingVar, candidates, numCand, indices);
// }
// public int solve (Twist vel, Point3d[] pnts, Vector3d[] nrms,
// double[] mus, int ncontacts,
// SpatialInertia inertia, Twist tw)
// {
// ContactInfo[] cinfo = new ContactInfo[ncontacts];
// for (int i=0; i<ncontacts; i++)
// { ContactInfo c = new ContactInfo();
// c.mu = mus[i];
// c.pnt = pnts[i];
// c.nrm = nrms[i];
// cinfo[i] = c;
// }
// Wrench wapplied = new Wrench();
// inertia.mul (wapplied, tw);
// return solve (vel, cinfo, ncontacts, inertia,
// wapplied, numFrictionDirs);
// }
/**
 * Applies an impulse to a rigid body velocity to adjust for normal and
 * frictional contact forces.
 *
 * @param tr
 * velocity with contact impulse added
 * @param contacts
 * provides information about the each contact
 * @param ncontacts
 * number of contacts
 * @param inertia
 * spatial inertia of the rigid body
 * @param tw
 * initial velocity of the rigid body
 * @param restitution
 * coefficent of restitution
 */
public int solve(Twist tr, ContactInfo[] contacts, int ncontacts, SpatialInertia inertia, Twist tw, double restitution) {
    if (ncontacts == 0) {
        tr.set(tw);
        return SOLVED;
    }
    Wrench wapplied = new Wrench();
    inertia.mul(wapplied, tw);
    return solve(tr, contacts, ncontacts, inertia, wapplied, restitution);
}
Also used : Wrench(maspack.spatialmotion.Wrench)

Example 10 with Wrench

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

the class RigidBody method getEffectiveMassForces.

public static int getEffectiveMassForces(VectorNd f, double t, FrameState state, SpatialInertia effectiveInertia, int idx) {
    Wrench coriolisForce = new Wrench();
    Twist bodyVel = new Twist();
    bodyVel.inverseTransform(state.XFrameToWorld.R, state.vel);
    SpatialInertia S = effectiveInertia;
    S.coriolisForce(coriolisForce, bodyVel);
    if (dynamicVelInWorldCoords) {
        coriolisForce.transform(state.XFrameToWorld.R);
    }
    double[] buf = f.getBuffer();
    buf[idx++] = -coriolisForce.f.x;
    buf[idx++] = -coriolisForce.f.y;
    buf[idx++] = -coriolisForce.f.z;
    buf[idx++] = -coriolisForce.m.x;
    buf[idx++] = -coriolisForce.m.y;
    buf[idx++] = -coriolisForce.m.z;
    return idx;
}
Also used : Twist(maspack.spatialmotion.Twist) Wrench(maspack.spatialmotion.Wrench) SpatialInertia(maspack.spatialmotion.SpatialInertia)

Aggregations

Wrench (maspack.spatialmotion.Wrench)11 Twist (maspack.spatialmotion.Twist)7 SpatialInertia (maspack.spatialmotion.SpatialInertia)4 Point3d (maspack.matrix.Point3d)3 RigidTransform3d (maspack.matrix.RigidTransform3d)2 Point (artisynth.core.mechmodels.Point)1 HasCoordinateFrame (artisynth.core.modelbase.HasCoordinateFrame)1 PolygonalMesh (maspack.geometry.PolygonalMesh)1 Matrix3d (maspack.matrix.Matrix3d)1 Matrix3x3Block (maspack.matrix.Matrix3x3Block)1 Matrix6d (maspack.matrix.Matrix6d)1 Matrix6dBlock (maspack.matrix.Matrix6dBlock)1 MatrixBlock (maspack.matrix.MatrixBlock)1 Quaternion (maspack.matrix.Quaternion)1 RotationMatrix3d (maspack.matrix.RotationMatrix3d)1