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

Example 16 with RigidBody

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

the class DoubleArmDemo method addEndPoint.

public void addEndPoint() {
    RigidBody thirdArm = model.rigidBodies().get("third");
    if (thirdArm == null) {
        return;
    }
    FrameMarker endPoint = new FrameMarker();
    endPoint.setName("endPoint");
    endPoint.setFrame(thirdArm);
    endPoint.setLocation(new Point3d(0, 0, len / 2));
    model.addFrameMarker(endPoint);
    // lowerArm.addMarker(endPoint);
    RenderProps rp = new RenderProps(model.getRenderProps());
    rp.setShading(Renderer.Shading.SMOOTH);
    rp.setPointColor(Color.ORANGE);
    rp.setPointRadius(len / 20);
    endPoint.setRenderProps(rp);
}

Example 17 with RigidBody

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

the class DoubleArmDemo method addBody.

public void addBody(String name, RigidTransform3d pose, String meshName) {
    // add a simple rigid body to the simulation
    RigidBody rb = new RigidBody();
    rb.setName(name);
    rb.setPose(pose);
    model.addRigidBody(rb);
    PolygonalMesh mesh;
    try {
        String meshFilename = ArtisynthPath.getHomeRelativePath("src/artisynth/demos/mech/geometry/", ".") + meshName;
        mesh = new PolygonalMesh();
        mesh.read(new BufferedReader(new FileReader(new File(meshFilename))));
        rb.setMesh(mesh, meshFilename);
    } catch (IOException e) {
        System.out.println(e.getMessage());
        mesh = MeshFactory.createBox(size.x, size.y, size.z);
        rb.setMesh(mesh, null);
    }
    rb.setInertia(SpatialInertia.createBoxInertia(10.0, size.x, size.y, size.z));
    RenderProps rp = new RenderProps(model.getRenderProps());
    rp.setFaceColor(Color.GRAY);
    rp.setShading(Renderer.Shading.FLAT);
    rb.setRenderProps(rp);
    rb.setFrameDamping(10);
    rb.setRotaryDamping(1000.0);
}

Example 18 with RigidBody

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

the class DoubleArmDemo method addMuscles.

public void addMuscles() {
    RigidBody upperArm = model.rigidBodies().get("upper");
    RigidBody lowerArm = model.rigidBodies().get("lower");
    RigidBody thirdArm = model.rigidBodies().get("third");
    if (upperArm == null || lowerArm == null || thirdArm == null) {
        return;
    }
    Point3d markerBodyPos = new Point3d(-size.x / 2, 0, (size.z / 2.0) / 1.2);
    FrameMarker u = new FrameMarker();
    model.addFrameMarker(u, upperArm, markerBodyPos);
    u.setName("upperAttachment");
    markerBodyPos = new Point3d(size.x / 2, 0, (size.z / 2.0) / 1.2);
    FrameMarker tu = new FrameMarker();
    model.addFrameMarker(tu, thirdArm, markerBodyPos);
    tu.setName("thirdUpperAttachment");
    markerBodyPos = new Point3d(size.x / 2, 0, -(size.z / 2.0) / 2);
    FrameMarker l = new FrameMarker();
    model.addFrameMarker(l, lowerArm, markerBodyPos);
    l.setName("lowerAttachment");
    markerBodyPos = new Point3d(size.x / 2, 0, (size.z / 2.0) / 2);
    FrameMarker tl = new FrameMarker();
    model.addFrameMarker(tl, lowerArm, markerBodyPos);
    tl.setName("thirdLowerAttachment");
    Muscle muscle = new Muscle("muscle");
    muscle.setPeckMuscleMaterial(20.0, 22.0, 30, 0.2, 0.5, 0.1);
    Muscle muscle2 = new Muscle("muscle2");
    muscle2.setPeckMuscleMaterial(8, 20, 30, 0.2, 0.5, 0.1);
    muscle.setFirstPoint(u);
    muscle2.setFirstPoint(tu);
    muscle.setSecondPoint(l);
    muscle2.setSecondPoint(tl);
    RenderProps rp = new RenderProps(model.getRenderProps());
    rp.setLineStyle(Renderer.LineStyle.SPINDLE);
    rp.setLineRadius(len / 20);
    // rp.setLineSlices(10);
    rp.setShading(Renderer.Shading.SMOOTH);
    rp.setLineColor(Color.RED);
    muscle.setRenderProps(rp);
    muscle2.setRenderProps(rp);
    model.addAxialSpring(muscle);
    model.addAxialSpring(muscle2);
    if (addCompression) {
        markerBodyPos = new Point3d(size.x / 2, 0, +size.z / 2.0);
        FrameMarker l2 = new FrameMarker();
        model.addFrameMarker(l2, lowerArm, markerBodyPos);
        l2.setName("lowerAttachmentCompressor");
        double len = u.getPosition().distance(l2.getPosition());
        AxialSpring s = new AxialSpring(10, 0, 50);
        s.setFirstPoint(u);
        s.setSecondPoint(l2);
        model.addAxialSpring(s);
        RenderProps props = new RenderProps();
        props.setLineStyle(Renderer.LineStyle.CYLINDER);
        props.setLineRadius(0.0);
        s.setRenderProps(props);
        // restoring spring
        len = tu.getPosition().distance(tl.getPosition());
        s = new AxialSpring(10, 0, 2 * len);
        s.setFirstPoint(tu);
        s.setSecondPoint(tl);
        model.addAxialSpring(s);
        s.setRenderProps(props);
    }
}

Example 19 with RigidBody

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

the class DoubleArmDemo method addJoint.

public void addJoint() {
    RigidBody upperArm = model.rigidBodies().get("upper");
    RigidBody lowerArm = model.rigidBodies().get("lower");
    RigidBody thirdArm = model.rigidBodies().get("third");
    if (upperArm == null || lowerArm == null || thirdArm == null) {
        return;
    }
    RevoluteJoint j = new RevoluteJoint();
    j.setName("elbow");
    RigidTransform3d TCA = new RigidTransform3d();
    TCA.p.z = -len / 2;
    TCA.R.setAxisAngle(1, 0, 0, Math.PI / 2);
    RigidTransform3d TCW = new RigidTransform3d();
    TCW.R.setAxisAngle(1, 0, 0, Math.PI / 2);
    j.setBodies(lowerArm, TCA, null, TCW);
    j.setAxisLength(len / 3);
    model.addBodyConnector(j);
    upperArm.setDynamic(false);
    // add joint between lower and third
    j = new RevoluteJoint();
    j.setName("wrist");
    TCA = new RigidTransform3d();
    TCA.p.z = -len / 2;
    TCA.R.setAxisAngle(1, 0, 0, Math.PI / 2);
    TCW = new RigidTransform3d();
    TCW.p.z = len / 2;
    TCW.R.setAxisAngle(1, 0, 0, Math.PI / 2);
    j.setBodies(thirdArm, TCA, lowerArm, TCW);
    j.setAxisLength(len / 3);
    model.addBodyConnector(j);
}

Example 20 with RigidBody

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

the class MechModelCollide method build.

public void build(String[] args) {
    MechModel mechMod = new MechModel("mechMod");
    // mechMod.setProfiling (true);
    mechMod.setGravity(0, 0, -98);
    // mechMod.setRigidBodyDamper (new FrameDamper (1.0, 4.0));
    mechMod.setFrameDamping(1.0);
    mechMod.setRotaryDamping(4.0);
    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 = 30;
    double leny0 = 30;
    double lenz0 = 2;
    RigidBody base = RigidBody.createBox("base", lenx0, leny0, lenz0, 0.2);
    base.setDynamic(false);
    mechMod.setDefaultCollisionBehavior(true, 0.20);
    RenderProps props;
    // 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);
    mechMod.addRigidBody(link1);
    // 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);
    mechMod.addRigidBody(link2);
    if (transparentLinks) {
        RenderProps.setFaceStyle(link1, Renderer.FaceStyle.NONE);
        RenderProps.setDrawEdges(link1, true);
        RenderProps.setFaceStyle(link2, Renderer.FaceStyle.NONE);
        RenderProps.setDrawEdges(link2, true);
    }
    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);
        mechMod.addBodyConnector(sjoint);
        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);
        rjoint.setName("joint2");
        rjoint.setAxisLength(4);
        RenderProps.setLineRadius(rjoint, 0.2);
        mechMod.addBodyConnector(rjoint);
        rjoint.setTheta(35);
        joint2 = rjoint;
    }
    mechMod.transformGeometry(new RigidTransform3d(-5, 0, 10, 1, 1, 0, Math.toRadians(30)));
    mechMod.addRigidBody(base);
    mechMod.setCollisionBehavior(link1, link2, false);
    CollisionManager cm = mechMod.getCollisionManager();
    RenderProps.setVisible(cm, true);
    RenderProps.setLineWidth(cm, 3);
    RenderProps.setLineColor(cm, Color.RED);
    cm.setDrawContactNormals(true);
    mechMod.setPenetrationTol(1e-3);
    // try {
    // MechSystemSolver.setLogWriter (
    // ArtisynthIO.newIndentingPrintWriter ("solve.txt"));
    // }
    // catch (Exception e)  {
    // }
    addModel(mechMod);
    // mechMod.setIntegrator (Integrator.ForwardEuler);
    // addBreakPoint (0.51);
    // mechMod.setProfiling (true);
    addControlPanel(mechMod);
}