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

Example 6 with RigidBody

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

the class Fem3dBlock method setConnected.

public synchronized void setConnected(boolean connect) {
    MechModel mechMod = (MechModel) findComponent("models/mech");
    if (mechMod != null) {
        FemModel3d femMod = (FemModel3d) mechMod.findComponent("models/fem");
        LinkedList<FemNode3d> rightNodes = new LinkedList<FemNode3d>();
        for (int i = 0; i < myAttachNodes.length; i++) {
            rightNodes.add(myAttachNodes[i]);
        }
        if (connect && !rightNodes.get(0).isAttached()) {
            RigidBody rightBody = (RigidBody) mechMod.findComponent("rigidBodies/rightBody");
            // position the block so that it lies at the current
            // end of the beam
            Plane plane = new Plane();
            Point3d centroid = new Point3d();
            int numPnts = rightNodes.size();
            Point3d[] pnts = new Point3d[numPnts];
            for (int i = 0; i < numPnts; i++) {
                pnts[i] = rightNodes.get(i).getPosition();
                centroid.add(pnts[i]);
            }
            centroid.scale(1 / (double) numPnts);
            plane.fit(pnts, numPnts);
            Vector3d normal = new Vector3d(plane.getNormal());
            // to determine the appropriate sign of the normal
            for (FemNode3d node : femMod.getNodes()) {
                if (!rightNodes.contains(node)) {
                    Vector3d diff = new Vector3d();
                    diff.sub(node.getPosition(), rightNodes.get(0).getPosition());
                    if (diff.dot(normal) > 0) {
                        normal.negate();
                    }
                    break;
                }
            }
            RigidTransform3d X = new RigidTransform3d();
            X.R.setZDirection(normal);
            X.R.mulAxisAngle(0, 1, 0, -Math.PI / 2);
            X.p.set(centroid);
            X.mulXyz(0.05, 0, 0);
            rightBody.setPose(X);
            rightBody.setVelocity(new Twist());
            for (FemNode3d n : rightNodes) {
                mechMod.attachPoint(n, rightBody);
            }
        } else if (!connect && rightNodes.get(0).isAttached()) {
            for (FemNode3d n : rightNodes) {
                mechMod.detachPoint(n);
            }
        }
    }
    myConnectedP = connect;
}

Example 7 with RigidBody

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

the class FemBeamMech method build.

public void build(String[] args) {
    femPath = "models/mech/models/fem/";
    modPath = "models/mech/";
    int nn = 2;
    myFemMod = FemFactory.createTetGrid(null, 0.6, 0.2, 0.2, nn * 3, nn * 1, nn * 1);
    myFemMod.setName("fem");
    myFemMod.setDensity(myDensity);
    myFemMod.setBounds(new Point3d(-0.6, 0, 0), new Point3d(0.6, 0, 0));
    myFemMod.setLinearMaterial(60000, 0.33, true);
    myFemMod.setStiffnessDamping(0.002);
    myFemMod.setImplicitIterations(100);
    myFemMod.setImplicitPrecision(0.001);
    myFemMod.setSurfaceRendering(SurfaceRender.Shaded);
    Renderable elems = myFemMod.getElements();
    RenderProps.setLineWidth(elems, 2);
    RenderProps.setLineColor(elems, Color.BLUE);
    Renderable nodes = myFemMod.getNodes();
    RenderProps.setPointStyle(nodes, Renderer.PointStyle.SPHERE);
    RenderProps.setPointRadius(nodes, 0.005);
    RenderProps.setPointColor(nodes, Color.GREEN);
    // fix the leftmost nodes
    double EPS = 1e-9;
    for (FemNode3d n : myFemMod.getNodes()) {
        if (n.getPosition().x < -0.3 + EPS) {
            myLeftNodes.add(n);
        }
    }
    System.out.println("fixed nodes:");
    for (FemNode3d n : myLeftNodes) {
        n.setDynamic(false);
    }
    RenderProps.setFaceColor(myFemMod, new Color(0.4f, 0.4f, 1.0f));
    myFemMod.setProfiling(true);
    RigidBody anchorBox = new RigidBody("anchorBox");
    PolygonalMesh mesh = MeshFactory.createBox(0.1, 0.3, 0.3);
    anchorBox.setMesh(mesh, /* fileName= */
    null);
    RigidTransform3d X = new RigidTransform3d();
    X.p.set(-0.35, 0, 0);
    anchorBox.setPose(X);
    anchorBox.setDynamic(false);
    myMechMod = new MechModel("mech");
    myMechMod.addModel(myFemMod);
    myMechMod.addRigidBody(anchorBox);
    System.out.println("models: " + myMechMod.findComponent("models"));
    System.out.println("models/fem: " + myMechMod.findComponent("models/fem"));
    myMechMod.setIntegrator(Integrator.BackwardEuler);
    addModel(myMechMod);
    myMechMod.setProfiling(true);
    // add marker to lower right corner element
    Point3d corner = new Point3d(0.3, -0.1, -0.1);
    FemElement cornerElem = null;
    for (FemElement e : myFemMod.getElements()) {
        FemNode[] nodeList = e.getNodes();
        for (int i = 0; i < nodeList.length; i++) {
            if (nodeList[i].getPosition().epsilonEquals(corner, 1e-8)) {
                cornerElem = e;
                break;
            }
        }
    }
    if (cornerElem != null) {
        FemMarker mkr = new FemMarker(0.3, -0.07, -0.03);
        myFemMod.addMarker(mkr, cornerElem);
        RenderProps.setPointStyle(mkr, Renderer.PointStyle.SPHERE);
        RenderProps.setPointRadius(mkr, 0.01);
        RenderProps.setPointColor(mkr, Color.WHITE);
        Particle part = new Particle(1, 0.5, -0.07, -0.03);
        RenderProps.setPointStyle(part, Renderer.PointStyle.SPHERE);
        RenderProps.setPointRadius(part, 0.01);
        part.setDynamic(false);
        myMechMod.addParticle(part);
        AxialSpring spr = new AxialSpring(1000, 0, 0);
        myMechMod.attachAxialSpring(part, mkr, spr);
        RenderProps.setLineStyle(spr, Renderer.LineStyle.SPINDLE);
        RenderProps.setLineRadius(spr, 0.01);
        RenderProps.setLineColor(spr, Color.GREEN);
    }
    int numWays = 0;
    double res = 0.2;
    for (int i = 0; i < numWays; i++) {
        addWayPoint(new WayPoint((i + 1) * res, true));
    }
    addControlPanel(myMechMod, myFemMod);
}

Example 8 with RigidBody

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

the class FemCollision method build.

public void build(String[] args) throws IOException {
    try {
        MechModel mechmod = new MechModel();
        // mechmod.setIntegrator (Integrator.ConstrainedBackwardEuler);
        mechmod.setIntegrator(Integrator.BackwardEuler);
        // mechmod.setProfiling (true);
        CollisionManager collisions = mechmod.getCollisionManager();
        RenderProps.setVisible(collisions, true);
        RenderProps.setEdgeWidth(collisions, 4);
        RenderProps.setEdgeColor(collisions, Color.YELLOW);
        RenderProps.setLineWidth(collisions, 3);
        RenderProps.setLineColor(collisions, Color.GREEN);
        collisions.setDrawContactNormals(true);
        RigidBody table = new RigidBody("table");
        table.setDynamic(false);
        // table.setMesh (new PolygonalMesh (new File (rbpath+ "box.obj")), null);
        table.setMesh(MeshFactory.createBox(2, 2, 2));
        AffineTransform3d trans = new AffineTransform3d();
        trans.setIdentity();
        trans.applyScaling(4, 2, 0.5);
        table.transformGeometry(trans);
        table.setPose(new RigidTransform3d(new Vector3d(1, 0, 0.8077474533228615), new AxisAngle(1, 0, 0, Math.toRadians(mu == 0 ? 0.0 : 1.5))));
        if (wireFrame) {
            RenderProps.setFaceStyle(table, Renderer.FaceStyle.NONE);
            RenderProps.setDrawEdges(table, true);
        }
        mechmod.addRigidBody(table);
        if (incBox0) {
            box0 = new RigidBody("box0");
            // box0.setMesh (
            // new PolygonalMesh (new File (rbpath + "box.obj")), null);
            box0.setMesh(MeshFactory.createBox(2, 2, 2));
            trans.setIdentity();
            trans.applyScaling(1.5, 1.5, 0.5);
            box0.transformGeometry(trans);
            box0.setInertia(SpatialInertia.createBoxInertia(10000, 4, 4, 1));
            box0.setPose(new RigidTransform3d(new Vector3d(-0.5, 0, 3.5), new AxisAngle()));
            RenderProps.setFaceColor(box0, Color.GREEN.darker());
            if (wireFrame) {
                RenderProps.setFaceStyle(box0, Renderer.FaceStyle.NONE);
                RenderProps.setDrawEdges(box0, true);
            }
            mechmod.addRigidBody(box0);
        }
        if (incFem0) {
            String fem0Name = // "torus546";
            "sphere2";
            // "box0023";
            // "box0048";
            // "box0144";
            // "box0604";
            // "box1056";
            // "box2463";
            // "box4257";
            fem0 = TetGenReader.read("fem0", 5000, fempath + fem0Name + ".1.node", fempath + fem0Name + ".1.ele", new Vector3d(0.8, 0.8, 0.8));
            fem0.transformGeometry(new RigidTransform3d(new Vector3d(2, 0, 3.5), new AxisAngle()));
            fem0.setLinearMaterial(1000000, 0.33, true);
            if (!wireFrame)
                fem0.setSurfaceRendering(SurfaceRender.Shaded);
            fem0.setParticleDamping(0.1);
            RenderProps.setFaceColor(fem0, new Color(0.5f, 0f, 0f));
            // RenderProps.setAlpha(fem0, 0.33);
            RenderProps.setAlpha(fem0, 0.5);
            RenderProps.setShading(fem0, Shading.NONE);
            RenderProps.setDrawEdges(fem0, true);
            RenderProps.setVisible(fem0.getElements(), false);
            RenderProps.setVisible(fem0.getNodes(), false);
            // RenderProps.setLineColor(fem0, Color.GRAY);
            mechmod.addModel(fem0);
        }
        if (incFem1) {
            // Use this code for a box
            /*
             * double mySize = 1; fem1 = createFem (name, mySize);
             * FemFactory.createTetGrid ( fem1, 1*mySize, 4*mySize, mySize, 1,
             * 1, 1);
             */
            // end box code
            // Use this code for a ball
            String fem1Name = "sphere2";
            fem1 = TetGenReader.read("fem1", 5000, fempath + fem1Name + ".1.node", fempath + fem1Name + ".1.ele", new Vector3d(0.8, 0.8, 0.8));
            fem1.setLinearMaterial(1000000, 0.33, true);
            fem1.setParticleDamping(0.1);
            // end ball code
            fem1.setIncompressible(FemModel3d.IncompMethod.AUTO);
            fem1.transformGeometry(new RigidTransform3d(new Vector3d(1.25, 0, 2), new AxisAngle()));
            fem1.setSurfaceRendering(wireFrame ? SurfaceRender.None : SurfaceRender.Shaded);
            RenderProps.setAlpha(fem1, 0.5);
            RenderProps.setShading(fem1, Shading.NONE);
            RenderProps.setDrawEdges(fem1, true);
            RenderProps.setVisible(fem1.getElements(), false);
            RenderProps.setVisible(fem1.getNodes(), false);
            RenderProps.setFaceColor(fem1, new Color(0f, 0f, 0.8f));
            mechmod.addModel(fem1);
        }
        if (incFem0) {
            mechmod.setCollisionBehavior(fem0, table, true, mu);
            mechmod.setCollisionResponse(fem0, Collidable.Deformable);
            mechmod.setCollisionResponse(fem0, table);
        }
        if (incFem1 & incFem0) {
            mechmod.setCollisionBehavior(fem0, fem1, true, mu);
        }
        if (incBox0 & incFem0) {
            mechmod.setCollisionBehavior(box0, fem0, true, mu);
        }
        if (incFem1) {
            mechmod.setCollisionBehavior(fem1, table, true, mu);
            mechmod.setCollisionResponse(fem1, Collidable.AllBodies);
        }
        if (incBox0 & incFem1) {
            mechmod.setCollisionBehavior(box0, fem1, true, mu);
        }
        if (incBox0) {
            mechmod.setCollisionBehavior(box0, table, true, mu);
            mechmod.setCollisionResponse(box0, table);
        }
        addModel(mechmod);
    // mechmod.setIntegrator (Integrator.BackwardEuler);
    // addBreakPoint (2.36);
    // reset();
    } catch (IOException e) {
        throw e;
    }
}

Example 9 with RigidBody

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

the class FemMuscleArm method setCollisions.

public void setCollisions(boolean collisions) {
    MechModel mech = (MechModel) models().get(0);
    PointToPointMuscle muscle = (PointToPointMuscle) mech.findComponent("models/0");
    RigidBody upperArm = (RigidBody) mech.findComponent("rigidBodies/upper");
    if (collisions) {
        mech.setCollisionBehavior(muscle, upperArm, true);
    } else {
        mech.setCollisionBehavior(muscle, upperArm, false);
    }
}

Example 10 with RigidBody

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

the class ArticulatedFem method build.

public void build(String[] args) {
    int nlinks = 3;
    int nelemsx = 6;
    int nelemsz = 2;
    double femLength = 0.6;
    double femHeight = 0.2;
    double boxLength = 0.1;
    double boxHeight = 0.3;
    double linkLength = femLength + 2 * boxLength;
    myMechMod = new MechModel("mech");
    RigidTransform3d X = new RigidTransform3d();
    RigidBody lastBox = null;
    double linkCenter;
    RigidBody leftAnchorBox = makeBox();
    linkCenter = linkLength * (-nlinks / 2.0 + 0.5);
    X.p.set(linkCenter - (boxLength + femLength) / 2, 0, boxHeight);
    leftAnchorBox.setPose(X);
    leftAnchorBox.setDynamic(false);
    // myMechMod.addRigidBody (leftAnchorBox);
    RigidBody rightAnchorBox = makeBox();
    linkCenter = linkLength * (-nlinks / 2.0 + (nlinks - 1) + 0.5);
    X.p.set(linkCenter + (boxLength + femLength) / 2, 0, boxHeight);
    rightAnchorBox.setPose(X);
    rightAnchorBox.setDynamic(false);
    for (int i = 0; i < nlinks; i++) {
        linkCenter = linkLength * (-nlinks / 2.0 + i + 0.5);
        LinkedList<FemNode3d> leftNodes = new LinkedList<FemNode3d>();
        LinkedList<FemNode3d> rightNodes = new LinkedList<FemNode3d>();
        FemModel3d femMod = FemFactory.createTetGrid(null, femLength, femHeight, femHeight, nelemsx, nelemsz, nelemsz);
        femMod.setDensity(myDensity);
        femMod.setLinearMaterial(200000, 0.4, true);
        femMod.setGravity(0, 0, -9.8);
        double eps = 0.000001;
        for (FemNode3d n : femMod.getNodes()) {
            double x = n.getPosition().x;
            if (x <= -femLength / 2 + eps) {
                leftNodes.add(n);
            } else if (x >= femLength / 2 - eps) {
                rightNodes.add(n);
            }
        }
        femMod.setStiffnessDamping(0.002);
        RenderProps.setLineWidth(femMod.getElements(), 2);
        RenderProps.setLineColor(femMod.getElements(), Color.BLUE);
        RenderProps.setPointStyle(femMod.getNodes(), Renderer.PointStyle.SPHERE);
        RenderProps.setPointRadius(femMod.getNodes(), 0.005);
        femMod.setSurfaceRendering(SurfaceRender.Shaded);
        RenderProps.setFaceColor(femMod, new Color(0f, 0.7f, 0.7f));
        RenderProps.setLineColor(femMod.getElements(), new Color(0f, 0.2f, 0.2f));
        RenderProps.setLineWidth(femMod.getElements(), 3);
        X.p.set(linkCenter, 0, 0);
        femMod.transformGeometry(X);
        myMechMod.addModel(femMod);
        RigidBody leftBox = makeBox();
        X.p.set(linkCenter - (boxLength + femLength) / 2, 0, 0);
        leftBox.setPose(X);
        myMechMod.addRigidBody(leftBox);
        RigidBody rightBox = makeBox();
        X.p.set(linkCenter + (boxLength + femLength) / 2, 0, 0);
        rightBox.setPose(X);
        myMechMod.addRigidBody(rightBox);
        for (FemNode3d n : leftNodes) {
            myMechMod.attachPoint(n, leftBox);
        }
        for (FemNode3d n : rightNodes) {
            myMechMod.attachPoint(n, rightBox);
        }
        RigidTransform3d TCA = new RigidTransform3d();
        RigidTransform3d TCB = new RigidTransform3d();
        RevoluteJoint joint;
        TCA.p.set(-boxLength / 2, 0, boxHeight / 2);
        TCA.R.mulAxisAngle(1, 0, 0, Math.PI / 2);
        if (lastBox == null) {
            TCB.mul(leftBox.getPose(), TCA);
            // TCB.mulInverseLeft (leftAnchorBox.getPose(), TCB);
            joint = new RevoluteJoint(leftBox, TCA, TCB);
        } else {
            TCB.p.set(boxLength / 2, 0, boxHeight / 2);
            TCB.R.mulAxisAngle(1, 0, 0, Math.PI / 2);
            joint = new RevoluteJoint(leftBox, TCA, lastBox, TCB);
        }
        RenderProps.setLineRadius(joint, 0.01);
        RenderProps.setLineColor(joint, new Color(0.15f, 0.15f, 1f));
        joint.setAxisLength(0.5);
        myMechMod.addBodyConnector(joint);
        if (addLastJoint && i == nlinks - 1) {
            TCA.p.set(boxLength / 2, 0, boxHeight / 2);
            TCB.mul(rightBox.getPose(), TCA);
            // TCB.mulInverseLeft (rightAnchorBox.getPose(), TCB);
            joint = new RevoluteJoint(rightBox, TCA, TCB);
            RenderProps.setLineRadius(joint, 0.01);
            RenderProps.setLineColor(joint, new Color(0.15f, 0.15f, 1f));
            joint.setAxisLength(0.5);
            myMechMod.addBodyConnector(joint);
        }
        lastBox = rightBox;
    }
    if (!addLastJoint) {
        lastBox.setDynamic(false);
    }
    myMechMod.setIntegrator(Integrator.BackwardEuler);
    addModel(myMechMod);
    addControlPanel(myMechMod);
}