Examples with Frame artisynth.core.mechmodels.Frame used on opensource projects

Example 1 with Frame

use of artisynth.core.mechmodels.Frame in project artisynth_core by artisynth.

the class FrameBodyAttachment method build.

public void build(String[] args) {
    // create MechModel and add to RootModel
    mech = new MechModel("mech");
    mech.setGravity(0, 0, -98);
    mech.setFrameDamping(1.0);
    mech.setRotaryDamping(4.0);
    addModel(mech);
    // bodies will be defined using a mesh
    PolygonalMesh mesh;
    // create bodyB and set its pose
    mesh = MeshFactory.createRoundedBox(lenx1, leny1, lenz1, /*nslices=*/
    8);
    RigidTransform3d TMB = new RigidTransform3d(0, 0, 0, /*axisAng=*/
    1, 1, 1, 2 * Math.PI / 3);
    mesh.transform(TMB);
    bodyB = RigidBody.createFromMesh("bodyB", mesh, /*density=*/
    0.2, 1.0);
    bodyB.setPose(new RigidTransform3d(0, 0, 1.5 * lenx1, 1, 0, 0, Math.PI / 2));
    // create bodyA and set its pose
    mesh = MeshFactory.createRoundedCylinder(leny2 / 2, lenx2, /*nslices=*/
    16, /*nsegs=*/
    1, /*flatBottom=*/
    false);
    mesh.transform(TMB);
    bodyA = RigidBody.createFromMesh("bodyA", mesh, 0.2, 1.0);
    bodyA.setPose(new RigidTransform3d((lenx1 + leny2) / 2, 0, 1.5 * lenx1, 0, Math.PI / 2, 0));
    // create the joint
    RigidTransform3d TDW = new RigidTransform3d(-lenx1 / 2, 0, 1.5 * lenx1, 1, 0, 0, Math.PI / 2);
    RevoluteJoint joint = new RevoluteJoint(bodyB, TDW);
    // add components to the mech model
    mech.addRigidBody(bodyB);
    mech.addRigidBody(bodyA);
    mech.addBodyConnector(joint);
    // set render properties for components
    RenderProps.setLineRadius(joint, 0.2);
    joint.setAxisLength(4);
    // now connect bodyA to bodyB using a FrameAttachment
    mech.attachFrame(bodyA, bodyB);
    // create an auxiliary frame and add it to the mech model
    Frame frame = new Frame();
    mech.addFrame(frame);
    // set the frames axis length > 0 so we can see it
    frame.setAxisLength(4.0);
    // set the attached frame's pose to that of bodyA ...
    RigidTransform3d TFW = new RigidTransform3d(bodyA.getPose());
    // ... plus a translation of lenx2/2 along the x axis:
    TFW.mulXyz(lenx2 / 2, 0, 0);
    // finally, attach the frame to bodyA
    mech.attachFrame(frame, bodyA, TFW);
}

Example 2 with Frame

use of artisynth.core.mechmodels.Frame in project artisynth_core by artisynth.

the class PointFem3dAttachment method collectMasters.

protected void collectMasters(List<DynamicComponent> masters) {
    super.collectMasters(masters);
    myFemFrame = null;
    if (myNodes != null) {
        // if so, then add that frame as a master
        for (int i = 0; i < myNodes.length; i++) {
            FemNode n = myNodes[i];
            Frame nframe = n.getPointFrame();
            if (nframe != null) {
                if (myFemFrame == null) {
                    myFemFrame = nframe;
                    masters.add(myFemFrame);
                } else if (myFemFrame != nframe) {
                    throw new UnsupportedOperationException("PointFem3d attachment not supported for multiple " + "frame-based FEMs");
                }
            }
        }
        // add nodes as master components as well
        for (int i = 0; i < myNodes.length; i++) {
            masters.add(myNodes[i]);
        }
    }
}

Example 3 with Frame

use of artisynth.core.mechmodels.Frame in project artisynth_core by artisynth.

the class PointFem3dAttachment method computeVelDerivative.

private boolean computeVelDerivative(Vector3d dvel) {
    Frame pframe = myPoint.getPointFrame();
    boolean isNonZero = false;
    // can we optimize this? If the attachment has been enforced, then can we
    // compute lp2 and lv2 directly from the point itself?
    Vector3d lv2 = new Vector3d();
    Vector3d lp2 = new Vector3d();
    double[] coords = myCoords.getBuffer();
    for (int i = 0; i < myNodes.length; i++) {
        FemNode node = myNodes[i];
        double w = coords[i];
        lv2.scaledAdd(w, node.getLocalVelocity());
        lp2.scaledAdd(w, node.getLocalPosition());
    }
    if (myFemFrame != null) {
        RotationMatrix3d R2 = myFemFrame.getPose().R;
        Twist vel2 = myFemFrame.getVelocity();
        Vector3d tmp1 = new Vector3d();
        Vector3d tmp2 = new Vector3d();
        // R2*lp2
        tmp1.transform(R2, lp2);
        // R2*lv2
        tmp2.transform(R2, lv2);
        // tmp1 = w2 X R2*lp2 + R2*lv2
        tmp1.crossAdd(vel2.w, tmp1, tmp2);
        // dvel = w2 X R2*lv2 + w2 X tmp1
        dvel.cross(vel2.w, tmp2);
        dvel.crossAdd(vel2.w, tmp1, dvel);
        if (pframe != null) {
            RotationMatrix3d R1 = pframe.getPose().R;
            Twist vel1 = pframe.getVelocity();
            // R1*lv1
            tmp2.transform(R1, myPoint.getLocalVelocity());
            tmp2.negate();
            // tmp2 = -R1*lv1 - u2 + u1 - tmp1
            tmp2.sub(vel2.v);
            tmp2.add(vel1.v);
            tmp2.sub(tmp1);
            // dvel = R1^T (w1 X tmp2 + dvel)
            dvel.crossAdd(vel1.w, tmp2, dvel);
            dvel.inverseTransform(R1);
        }
        isNonZero = true;
    } else if (pframe != null) {
        RotationMatrix3d R1 = pframe.getPose().R;
        Twist vel1 = pframe.getVelocity();
        // dvel = R1^T (w1 X (u1 - R1*lv1 - lv2))
        // R1*lv1
        dvel.transform(R1, myPoint.getLocalVelocity());
        dvel.negate();
        // since Fem has no frame, lv2 and world velocity are the same
        dvel.sub(lv2);
        dvel.add(vel1.v);
        dvel.cross(vel1.w, dvel);
        dvel.inverseTransform(R1);
        isNonZero = true;
    }
    return isNonZero;
}

Example 4 with Frame

use of artisynth.core.mechmodels.Frame in project artisynth_core by artisynth.

the class SkinMeshBody method postscanItem.

protected boolean postscanItem(Deque<ScanToken> tokens, CompositeComponent ancestor) throws IOException {
    if (postscanAttributeName(tokens, "frames")) {
        for (int i = 0; i < myFrameInfo.size(); i++) {
            Frame frame = postscanReference(tokens, Frame.class, ancestor);
            myFrameInfo.get(i).setFrame(frame);
        }
        return true;
    } else if (postscanAttributeName(tokens, "femModels")) {
        for (int i = 0; i < myFemModelInfo.size(); i++) {
            FemModel3d fem = postscanReference(tokens, FemModel3d.class, ancestor);
            myFemModelInfo.get(i).myFemModel = fem;
        }
        return true;
    }
    return super.postscanItem(tokens, ancestor);
}

Example 5 with Frame

use of artisynth.core.mechmodels.Frame in project artisynth_core by artisynth.

the class FemNode3d method setFrame.

public void setFrame(Frame frame) {
    Frame oldFrame = myPointFrame;
    super.setPointFrame(frame);
    if (oldFrame != frame) {
        // need to reset rest position
        if (oldFrame != null) {
            myRest.transform(oldFrame.getPose());
        }
        if (frame != null) {
            myRest.inverseTransform(frame.getPose());
        }
    }
// no need to invalidate stress, etc. since setFrame() will only be
// called by FemModel, which will take care of that
}