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

Example 26 with RigidBody

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

the class Main method setDragger.

private void setDragger() {
    translator3d.setVisible(false);
    transrotator3d.setVisible(false);
    scalar3d.setVisible(false);
    rotator3d.setVisible(false);
    constrainedTranslator3d.setVisible(false);
    constrainedTranslator3d.setMesh(null);
    currentDragger = null;
    myDraggableComponents.clear();
    if (mySelectionMode != SelectionMode.Select && mySelectionMode != SelectionMode.EllipticSelect && mySelectionMode != SelectionMode.Pull) {
        Point3d pmin = new Point3d(inf, inf, inf);
        Point3d pmax = new Point3d(-inf, -inf, -inf);
        int n = 0;
        for (ModelComponent sel : mySelectionManager.getCurrentSelection()) {
            if (sel instanceof Renderable && sel instanceof TransformableGeometry && !ComponentUtils.isAncestorSelected(sel)) {
                myDraggableComponents.add(sel);
                ((Renderable) sel).updateBounds(pmin, pmax);
                n++;
            }
        }
        if (n > 0) {
            RigidTransform3d TDW = new RigidTransform3d();
            double radius = computeDraggerToWorld(TDW, myDraggableComponents, null);
            // set a minimum radius to about 1/6 of the viewer window width
            radius = Math.max(radius, myViewer.distancePerPixel(myViewer.getCenter()) * myViewer.getScreenWidth() / 6);
            if (mySelectionMode == SelectionMode.Translate) {
                translator3d.setVisible(true);
                translator3d.setDraggerToWorld(TDW);
                translator3d.setSize(radius);
                currentDragger = translator3d;
            } else if (mySelectionMode == SelectionMode.Transrotate) {
                transrotator3d.setVisible(true);
                transrotator3d.setDraggerToWorld(TDW);
                transrotator3d.setSize(radius);
                currentDragger = transrotator3d;
            } else if (mySelectionMode == SelectionMode.Scale) {
                scalar3d.setVisible(true);
                scalar3d.setDraggerToWorld(TDW);
                scalar3d.setSize(radius);
                currentDragger = scalar3d;
            } else if (mySelectionMode == SelectionMode.Rotate) {
                rotator3d.setVisible(true);
                rotator3d.setDraggerToWorld(TDW);
                rotator3d.setSize(radius);
                currentDragger = rotator3d;
            } else if (mySelectionMode == SelectionMode.ConstrainedTranslate) {
                PolygonalMesh mesh = null;
                for (Object sel : mySelectionManager.getCurrentSelection()) {
                    if (sel instanceof FrameMarker) {
                        FrameMarker frameMarker = (FrameMarker) sel;
                        Point3d p = new Point3d(frameMarker.getPosition());
                        constrainedTranslator3d.setLocation(p);
                        Frame frame = frameMarker.getFrame();
                        if (frame instanceof RigidBody) {
                            mesh = ((RigidBody) frame).getMesh();
                        }
                        break;
                    }
                }
                if (mesh != null) {
                    constrainedTranslator3d.setSize(radius);
                    constrainedTranslator3d.setMesh(mesh);
                    constrainedTranslator3d.setVisible(true);
                    currentDragger = constrainedTranslator3d;
                }
            }
        }
    }
}

Example 27 with RigidBody

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

the class TorusWrapTest method build.

public void build(String[] args) {
    MechModel mechMod = new MechModel("mechMod");
    mechMod.setGravity(0, 0, -9.8);
    mechMod.setFrameDamping(10.0);
    mechMod.setRotaryDamping(0.1);
    RigidBody block = new RigidBody("block");
    PolygonalMesh mesh = MeshFactory.createBox(size, size, size);
    block.setMesh(mesh, null);
    block.setInertiaFromDensity(1);
    // mechMod.addRigidBody (block);
    Particle p0 = new Particle(0.1, size * 3, 0, size / 2);
    p0.setDynamic(false);
    mechMod.addParticle(p0);
    Particle p1 = new Particle(0.1, -size * 3, 0, size / 2);
    p1.setDynamic(false);
    mechMod.addParticle(p1);
    Particle p2 = new Particle(0.1, -size * 3, size, size / 2);
    p2.setDynamic(false);
    // mechMod.addParticle (p2);
    Particle p3 = new Particle(0.1, size * 3, size, size / 2);
    p3.setDynamic(false);
    // mechMod.addParticle (p3);
    RigidTorus torus1 = new RigidTorus("torus1", size, size / 3, myDensity, 50, 30);
    RigidTorus torus2 = new RigidTorus("torus2", size, size / 3, myDensity, 50, 30);
    RigidTorus torus3 = new RigidTorus("torus3", size, size / 3, myDensity, 50, 30);
    torus1.setPose(new RigidTransform3d(-2 * size, 0, 0, 0, Math.PI / 2, 0));
    torus2.setPose(new RigidTransform3d(2 * size, 0, 0, 0, Math.PI / 2, 0));
    torus3.setPose(new RigidTransform3d(0, 0, 0, 0, Math.PI / 2, 0));
    mechMod.addRigidBody(torus1);
    mechMod.addRigidBody(torus2);
    mechMod.addRigidBody(torus3);
    mechMod.setDefaultCollisionBehavior(true, 0);
    MultiPointSpring spring = new MultiPointSpring("spring", 100, 0.1, 0);
    spring.addPoint(p0);
    spring.setSegmentWrappable(50);
    spring.addWrappable(torus1);
    spring.addWrappable(torus2);
    spring.addWrappable(torus3);
    spring.addPoint(p1);
    // spring.addPoint (p2);
    // spring.setSegmentWrappable (50);
    // spring.addPoint (p3);
    // spring.setWrapDamping (1.0);
    // spring.setWrapStiffness (10);
    // spring.setWrapH (0.01);
    mechMod.addMultiPointSpring(spring);
    spring.setDrawKnots(false);
    spring.setDrawABPoints(true);
    spring.setWrapDamping(100);
    spring.setMaxWrapIterations(10);
    addModel(mechMod);
    RenderProps.setPointStyle(mechMod, Renderer.PointStyle.SPHERE);
    RenderProps.setPointRadius(mechMod, size / 10);
    RenderProps.setLineStyle(mechMod, Renderer.LineStyle.CYLINDER);
    RenderProps.setLineRadius(mechMod, size / 30);
    RenderProps.setLineColor(mechMod, Color.red);
    createControlPanel(mechMod);
}

Example 28 with RigidBody

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

the class MechModelDemo method build.

public void build(String[] args) {
    myMech = new MechModel("mechMod");
    // mechMod.setProfiling (true);
    myMech.setGravity(0, 0, -50);
    // myMech.setRigidBodyDamper (new FrameDamper (1.0, 4.0));
    myMech.setFrameDamping(1.0);
    myMech.setRotaryDamping(4.0);
    myMech.setIntegrator(MechSystemSolver.Integrator.SymplecticEuler);
    RigidTransform3d XMB = new RigidTransform3d();
    RigidTransform3d XLW = new RigidTransform3d();
    RigidTransform3d TCA = new RigidTransform3d();
    RigidTransform3d TCB = new RigidTransform3d();
    RigidTransform3d XAB = new RigidTransform3d();
    PolygonalMesh mesh;
    // number of slices for approximating a circle
    int nslices = 16;
    // // set view so tha points upwards
    // X.R.setAxisAngle (1, 0, 0, -Math.PI/2);
    // viewer.setTransform (X);
    double lenx0 = 15;
    double leny0 = 15;
    double lenz0 = 1.5;
    RigidBody base = new RigidBody("base");
    base.setInertia(SpatialInertia.createBoxInertia(10, lenx0, leny0, lenz0));
    mesh = MeshFactory.createBox(lenx0, leny0, lenz0);
    // XMB.setIdentity();
    // XMB.R.setAxisAngle (1, 1, 1, 2*Math.PI/3);
    // mesh.transform (XMB);
    // mesh.setRenderMaterial (Material.createSpecial (Material.GRAY));
    base.setMesh(mesh, /* fileName= */
    null);
    XLW.setIdentity();
    XLW.p.set(0, 0, 22);
    base.setPose(XLW);
    base.setDynamic(false);
    myMech.addRigidBody(base);
    RenderProps props;
    FrameMarker mk0 = new FrameMarker();
    props = mk0.createRenderProps();
    // props.setColor (Color.GREEN);
    props.setPointRadius(0.5);
    props.setPointStyle(Renderer.PointStyle.SPHERE);
    mk0.setRenderProps(props);
    myMech.addFrameMarker(mk0, base, new Point3d(lenx0 / 2, leny0 / 2, 0));
    FrameMarker mk1 = new FrameMarker();
    mk1.setRenderProps(props);
    myMech.addFrameMarker(mk1, base, new Point3d(-lenx0 / 2, -leny0 / 2, 0));
    FrameMarker mk2 = new FrameMarker();
    mk2.setRenderProps(props);
    FrameMarker mk3 = new FrameMarker();
    mk3.setRenderProps(props);
    double ks = 10;
    double ds = 10;
    AxialSpring spr0 = new AxialSpring(50, 10, 0);
    AxialSpring spr1 = new AxialSpring(50, 10, 0);
    props = spr0.createRenderProps();
    props.setLineStyle(Renderer.LineStyle.CYLINDER);
    props.setLineRadius(0.2);
    props.setLineColor(Color.RED);
    spr0.setRenderProps(props);
    spr1.setRenderProps(props);
    // myMech.addRigidBody (base);
    // first link
    double lenx1 = 10;
    double leny1 = 2;
    double lenz1 = 3;
    RigidBody link1 = new RigidBody("link1");
    link1.setInertia(SpatialInertia.createBoxInertia(10, lenx1, leny1, lenz1));
    mesh = MeshFactory.createRoundedBox(lenx1, leny1, lenz1, nslices / 2);
    XMB.setIdentity();
    XMB.R.setAxisAngle(1, 1, 1, 2 * Math.PI / 3);
    mesh.transform(XMB);
    // mesh.setRenderMaterial (Material.createSpecial (Material.GRAY));
    link1.setMesh(mesh, /* fileName= */
    null);
    XLW.R.setAxisAngle(1, 0, 0, Math.PI / 2);
    // XLW.R.mulAxisAngle (0, 1, 0, Math.PI/4);
    XLW.p.set(0, 0, 1.5 * lenx1);
    link1.setPose(XLW);
    myMech.addRigidBody(link1);
    myMech.addFrameMarker(mk2, link1, new Point3d(-lenx1 / 2, 0, -lenz1 / 2));
    myMech.addFrameMarker(mk3, link1, new Point3d(-lenx1 / 2, 0, lenz1 / 2));
    // // joint 1
    // if (usePlanarJoint)
    // {
    // TCA.setIdentity();
    // TCA.p.set (-lenx1/2, 0, 0);
    // TCA.R.setAxisAngle (1, 0, 0, -Math.PI/2);
    // TCB.p.set (0, 0, lenx1);
    // // TCB.R.setAxisAngle (1, 0, 0, -Math.PI/2);
    // PlanarConnector planar =
    // new PlanarConnector (link1, TCA.p, TCB);
    // planar.setName ("plane1");
    // planar.setPlaneSize (20);
    // RenderProps.setColor (planar, Color.BLUE);
    // joint1 = planar;
    // }
    // else
    // {
    // TCA.setIdentity();
    // TCA.p.set (-lenx1/2, 0, 0);
    // // TCA.R.mulAxisAngle (0, 1, 0, Math.PI/4);
    // TCB.set (link1.myState.XFrameToWorld);
    // TCB.mul (TCA);
    // RevoluteJoint rjoint = new RevoluteJoint (link1, TCA, TCB);
    // rjoint.setName ("joint1");
    // rjoint.setAxisLength (4);
    // RenderProps.setLineRadius(rjoint, 0.2);
    // joint1 = rjoint;
    // // SphericalJoint sjoint = new SphericalJoint (
    // // link1, TCA, TCB);
    // // sjoint.setName ("joint1");
    // // sjoint.setAxisLength (5);
    // // joint1 = sjoint;
    // }
    // second link
    double lenx2 = 10;
    double leny2 = 2;
    double lenz2 = 2;
    RigidBody link2 = new RigidBody("link2");
    if (// useSphericalJoint)
    false) {
        mesh = MeshFactory.createRoundedCylinder(leny2 / 2, lenx2, nslices, /*nsegs=*/
        1, /*flatBottom=*/
        false);
        link2.setInertia(SpatialInertia.createBoxInertia(10, leny2, leny2, lenx2));
        XLW.R.setAxisAngle(1, 0, 0, Math.PI / 2);
        XLW.p.set(lenx1 / 2, lenx2 / 2, lenx1);
        link2.setPose(XLW);
    }
    mesh = MeshFactory.createRoundedCylinder(leny2 / 2, lenx2, nslices, /*nsegs=*/
    1, /*flatBottom=*/
    false);
    mesh.transform(XMB);
    link2.setInertia(SpatialInertia.createBoxInertia(10, lenx2, leny2, lenz2));
    XLW.R.setAxisAngle(1, 0, 0, Math.PI / 2);
    XLW.p.set(lenx1 / 2 + lenx2 / 2, 0, 1.5 * lenx1);
    if (useSphericalJoint) {
        // Math.PI/4;
        double ang = 0;
        XLW.R.mulAxisAngle(0, 1, 0, ang);
        XLW.p.y += Math.sin(ang) * lenx2 / 2;
        XLW.p.x -= (1 - Math.cos(ang)) * lenx2 / 2;
    }
    link2.setPose(XLW);
    link2.setMesh(mesh, /* fileName= */
    null);
    myMech.addRigidBody(link2);
    BodyConnector joint2 = null;
    // joint 2
    if (useSphericalJoint) {
        TCA.setIdentity();
        TCA.p.set(-lenx2 / 2, 0, 0);
        XAB.mulInverseLeft(link1.getPose(), link2.getPose());
        TCB.mul(XAB, TCA);
        SphericalJoint sjoint = new SphericalJoint(link2, TCA, link1, TCB);
        // RevoluteJoint joint2 = new RevoluteJoint (link2, TCA, TCB);
        sjoint.setName("joint2");
        // RenderProps.setLineRadius(sjoint, 0.2);
        sjoint.setAxisLength(4);
        joint2 = sjoint;
    } else {
        TCA.setIdentity();
        TCA.p.set(-lenx2 / 2, 0, 0);
        // TCA.R.mulAxisAngle (1, 0, 0, -Math.toRadians(90));
        XAB.mulInverseLeft(link1.getPose(), link2.getPose());
        TCB.mul(XAB, TCA);
        RevoluteJoint rjoint = new RevoluteJoint(link2, TCA, link1, TCB);
        // TCB.mul (link2.getPose(), TCA);
        // RevoluteJoint rjoint =
        // new RevoluteJoint (link2, TCA, TCB);
        // RigidTransform3d X = new RigidTransform3d();
        // X.R.setAxisAngle (1, 0, 0, -Math.toRadians(90));
        // X.mul (TCB, X);
        // X.mulInverseRight (X, TCB);
        // rjoint.transformGeometry (X);
        // rjoint.printData();
        rjoint.setName("joint2");
        rjoint.setAxisLength(4);
        RenderProps.setLineRadius(rjoint, 0.2);
        // RigidTransform3d X = new RigidTransform3d();
        // RigidTransform3d TDW = rjoint.getXDW();
        // System.out.println ("getXDW=\n" + TDW.toString("%8.3f"));
        // X.R.setAxisAngle (1, 0, 0, Math.toRadians(80));
        // X.mulInverseRight (X, TDW);
        // X.mul (TDW, X);
        // rjoint.transformGeometry (X, rjoint);
        joint2 = rjoint;
    }
    // myMech.addBodyConnector (joint1);
    if (joint2 != null) {
        myMech.addBodyConnector(joint2);
    }
    myMech.attachAxialSpring(mk0, mk2, spr0);
    myMech.attachAxialSpring(mk1, mk3, spr1);
    if (usePlanarContacts) {
        TCA.setIdentity();
        TCA.p.set(lenx2 / 2 + leny2 / 2, 0, 0);
        TCB.setIdentity();
        // TCB.p.set (0, 0, -lenx2/2);
        // TCB.p.set (0, 0, lenx2/2);
        TCB.R.setIdentity();
        TCB.R.setAxisAngle(0, 0, 1, Math.PI / 2);
        TCB.R.mulAxisAngle(1, 0, 0, Math.toRadians(20));
        PlanarConnector contact1 = new PlanarConnector(link2, TCA.p, TCB);
        contact1.setUnilateral(true);
        contact1.setName("contact1");
        contact1.setPlaneSize(20);
        RenderProps.setFaceColor(contact1, new Color(0.5f, 0.5f, 1f));
        RenderProps.setAlpha(contact1, 0.5);
        myMech.addBodyConnector(contact1);
        TCB.R.setIdentity();
        TCB.R.setAxisAngle(0, 0, 1, Math.PI / 2);
        TCB.R.mulAxisAngle(1, 0, 0, -Math.toRadians(20));
        PlanarConnector contact2 = new PlanarConnector(link2, TCA.p, TCB);
        contact2.setUnilateral(true);
        contact2.setName("contact2");
        contact2.setPlaneSize(20);
        RenderProps.setFaceColor(contact2, new Color(0.5f, 0.5f, 1f));
        RenderProps.setAlpha(contact2, 0.5);
        myMech.addBodyConnector(contact2);
    }
    myMech.setBounds(new Point3d(0, 0, -10), new Point3d(0, 0, 10));
    addModel(myMech);
    addControlPanel(myMech);
// RigidTransform3d X = new RigidTransform3d (link1.getPose());
// X.R.mulRpy (Math.toRadians(-10), 0, 0);
// link1.setPose (X);
// myMech.projectRigidBodyPositionConstraints();
// myMech.setProfiling (true);
// myMech.setIntegrator (Integrator.ForwardEuler);
// addBreakPoint (0.57);
}

Example 29 with RigidBody

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

the class MultiMuscleDemo method build.

public void build(String[] args) {
    super.build(args);
    MechModel mechMod = (MechModel) models().get("mechMod");
    RigidBody block = mechMod.rigidBodies().get("block");
    Particle p0 = new Particle(0.1, 0, -size * 3, size / 2);
    p0.setDynamic(false);
    mechMod.addParticle(p0);
    Particle p1 = new Particle(0.1, 0, size * 3, size / 2);
    p1.setDynamic(false);
    mechMod.addParticle(p1);
    FrameMarker mkr0 = new FrameMarker();
    mechMod.addFrameMarker(mkr0, block, new Point3d(0, -size / 2, size / 2));
    FrameMarker mkr1 = new FrameMarker();
    mechMod.addFrameMarker(mkr1, block, new Point3d(0, size / 2, size / 2));
    MultiPointMuscle muscle = new MultiPointMuscle();
    LinearAxialMuscle mat = new LinearAxialMuscle();
    mat.setForceScaling(1);
    mat.setMaxForce(10);
    mat.setMaxLength(size);
    mat.setPassiveFraction(0.1);
    muscle.setMaterial(mat);
    muscle.addPoint(p0);
    muscle.addPoint(mkr0);
    muscle.addPoint(mkr1);
    muscle.addPoint(p1);
    mechMod.addMultiPointSpring(muscle);
    RenderProps.setLineColor(mechMod, Color.BLUE);
    RenderProps.setLineStyle(muscle, LineStyle.SPINDLE);
    RenderProps.setLineRadius(muscle, 0.1);
    RenderProps.setLineColor(muscle, Color.RED);
}

Example 30 with RigidBody

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

the class RigidBodyCollision method build.

public void build(String[] args) throws IOException {
    try {
        mechmod = new MechModel();
        mechmod.setMaxStepSize(0.005);
        boxes = new ArrayList<RigidBody>();
        table = new RigidBody("table");
        table.setDynamic(false);
        table.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        AffineTransform3d trans = new AffineTransform3d();
        trans.setIdentity();
        trans.applyScaling(4, 2, 0.5);
        table.transformGeometry(trans);
        table.setPose(new RigidTransform3d(new Vector3d(0, 0, 0.8077474533228615), new AxisAngle()));
        table.setInertia(SpatialInertia.createBoxInertia(1, 1, 1, 1));
        mechmod.addRigidBody(table);
        boxes.add(table);
        if (wireFrame) {
            setWireFrame(table);
        }
        // middle box in pile
        box0 = new RigidBody("box0");
        box0.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.5, 0.5, 0.5);
        box0.transformGeometry(trans);
        box0.setInertia(SpatialInertia.createBoxInertia(4, 1, 1, 1));
        addBox(box0, Color.GREEN);
        if (wireFrame) {
            setWireFrame(box0);
        }
        // long thin box, bottom of pile
        box1 = new RigidBody("box1");
        box1.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.6, 0.1, 1.9);
        box1.transformGeometry(trans);
        box1.setInertia(SpatialInertia.createBoxInertia(1, 1, 0.1, 4));
        addBox(box1, Color.YELLOW);
        if (wireFrame) {
            setWireFrame(box1);
        }
        // left hand box falling on unsupported end of
        box2 = new RigidBody("box2");
        // box1
        box2.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.5, 0.5, 0.5);
        box2.transformGeometry(trans);
        box2.setInertia(SpatialInertia.createBoxInertia(20, 1, 1, 1));
        addBox(box2, Color.BLUE);
        if (wireFrame) {
            setWireFrame(box2);
        }
        // top box in pile
        box3 = new RigidBody("box3");
        box3.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.4, 0.4, 0.4);
        box3.transformGeometry(trans);
        box3.setInertia(SpatialInertia.createBoxInertia(0.5, 0.5, 0.5, 4));
        addBox(box3, Color.CYAN);
        // box3.getMesh().name = "box3";
        if (wireFrame) {
            setWireFrame(box3);
        }
        // solo box off to the right.
        box4 = new RigidBody("box4");
        box4.setMesh(new PolygonalMesh(new File(rbpath + "box.obj")), null);
        trans.setIdentity();
        trans.applyScaling(0.6, 0.6, 0.3);
        box4.transformGeometry(trans);
        box4.setInertia(SpatialInertia.createBoxInertia(0.5, 0.5, 0.5, 4));
        box4.setPose(new RigidTransform3d(new Vector3d(1, 0.0, 5), new AxisAngle(0, 0, 0, 0)));
        addBox(box4, Color.RED);
        // box4.getMesh().name = "box4";
        if (wireFrame) {
            setWireFrame(box4);
        }
        mechmod.setDefaultCollisionBehavior(true, 0.05);
        reset();
        addModel(mechmod);
        ControlPanel panel = new ControlPanel();
        panel.addWidget(mechmod, "integrator");
        panel.addWidget(mechmod, "maxStepSize");
        addControlPanel(panel);
        Main.getMain().arrangeControlPanels(this);
        CollisionManager cm = mechmod.getCollisionManager();
        RenderProps.setVisible(cm, true);
        RenderProps.setLineWidth(cm, 3);
        RenderProps.setLineColor(cm, Color.RED);
        cm.setDrawContactNormals(true);
        // addBreakPoint (0.74);
        for (int i = 1; i <= 10; i++) {
            addWayPoint(0.1 * i);
        }
    // setWaypointChecking (true);
    } catch (IOException e) {
        throw e;
    }
}