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

Example 6 with RevoluteJoint

use of artisynth.core.mechmodels.RevoluteJoint 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);
}

Example 7 with RevoluteJoint

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

the class CompliantConstraintDemo method build.

public void build(String[] args) {
    MechModel mechMod = new MechModel("mechMod");
    // mechMod.setProfiling (true);
    mechMod.setGravity(0, 0, -50);
    // mechMod.setRigidBodyDamper (new FrameDamper (1.0, 4.0));
    mechMod.setFrameDamping(1.0);
    mechMod.setRotaryDamping(4.0);
    mechMod.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;
    RenderProps props;
    double ks = 10;
    double ds = 10;
    // mechMod.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);
    mechMod.addRigidBody(link1);
    // joint 1
    BodyConnector joint1 = null;
    RevoluteJoint rjoint = null;
    TCA.setIdentity();
    TCA.p.set(-lenx1 / 2, 0, 0);
    // TCA.R.mulAxisAngle (0, 1, 0, Math.PI/4);
    TCB.set(link1.getPose());
    TCB.mul(TCA);
    rjoint = new RevoluteJoint(link1, TCA, TCB);
    rjoint.setName("joint1");
    rjoint.setAxisLength(4);
    RenderProps.setLineRadius(rjoint, 0.2);
    joint1 = rjoint;
    // 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);
    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);
        rjoint = new RevoluteJoint(link2, TCA, link1, TCB);
        rjoint.setName("joint2");
        rjoint.setAxisLength(4);
        RenderProps.setLineRadius(rjoint, 0.2);
        joint2 = rjoint;
    }
    mechMod.addBodyConnector(joint1);
    if (joint2 != null) {
        mechMod.addBodyConnector(joint2);
    }
    mechMod.transformGeometry(new RigidTransform3d(0, 0, 0, 0, Math.toRadians(75), 0));
    addModel(mechMod);
    addControlPanel(mechMod);
}

Example 8 with RevoluteJoint

use of artisynth.core.mechmodels.RevoluteJoint 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 9 with RevoluteJoint

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

the class RigidBodyConnectorList method createAndAddConnector.

private void createAndAddConnector(Point3d origin) {
    BodyConnector connector;
    RigidTransform3d TCW = new RigidTransform3d();
    TCW.R.set(myBodyA.getPose().R);
    TCW.p.set(origin);
    RigidTransform3d TCA = new RigidTransform3d();
    TCA.mulInverseLeft(myBodyA.getPose(), TCW);
    if (myComponentType == RevoluteJoint.class) {
        RevoluteJoint joint;
        if (myBodyB == null) {
            joint = new RevoluteJoint(myBodyA, TCA, TCW);
        } else {
            RigidTransform3d TCB = new RigidTransform3d();
            TCB.mulInverseLeft(myBodyB.getPose(), TCW);
            joint = new RevoluteJoint(myBodyA, TCA, myBodyB, TCB);
        }
        connector = joint;
    } else if (myComponentType == SphericalJoint.class) {
        SphericalJoint joint;
        if (myBodyB == null) {
            joint = new SphericalJoint(myBodyA, TCA, TCW);
        } else {
            RigidTransform3d TCB = new RigidTransform3d();
            TCB.mulInverseLeft(myBodyB.getPose(), TCW);
            joint = new SphericalJoint(myBodyA, TCA, myBodyB, TCB);
        }
        connector = joint;
    } else {
        throw new InternalErrorException("Unimplemented connector type " + myComponentType);
    }
    connector.setName(getNameFieldValue());
    setProperties(connector, getPrototypeComponent(myComponentType));
    // update properties in the prototype as well ...
    setProperties(myPrototype, myPrototype);
    addComponent(new AddComponentsCommand("add BodyConnector", connector, (MutableCompositeComponent<?>) myModel.bodyConnectors()));
    setState(State.Complete);
    myMain.setSelectionMode(Main.SelectionMode.Translate);
}

Example 10 with RevoluteJoint

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

the class Fem3dBlock method build.

public void build(String type, int nz, int nxy, int options) {
    FemModel3d femMod = new FemModel3d("fem");
    femMod.setDensity(myDensity);
    if (type.equals("tet")) {
        FemFactory.createTetGrid(femMod, 0.6, 0.2, 0.2, nz, nxy, nxy);
    } else if (type.equals("hex")) {
        FemFactory.createHexGrid(femMod, 0.6, 0.2, 0.2, nz, nxy, nxy);
    } else if (type.equals("quadtet")) {
        FemFactory.createQuadtetGrid(femMod, 0.6, 0.2, 0.2, nz, nxy, nxy);
    } else if (type.equals("quadhex")) {
        FemFactory.createQuadhexGrid(femMod, 0.6, 0.2, 0.2, nz, nxy, nxy);
    } else {
        throw new UnsupportedOperationException("Unsupported element type " + type);
    }
    femMod.setBounds(new Point3d(-0.6, 0, 0), new Point3d(0.6, 0, 0));
    femMod.setLinearMaterial(200000, 0.4, true);
    femMod.setStiffnessDamping(0.002);
    Renderable elements = femMod.getElements();
    RenderProps.setLineWidth(elements, 2);
    RenderProps.setLineColor(elements, Color.BLUE);
    Renderable nodes = femMod.getNodes();
    RenderProps.setPointStyle(nodes, Renderer.PointStyle.SPHERE);
    RenderProps.setPointRadius(nodes, 0.01);
    RenderProps.setPointColor(nodes, new Color(153, 0, 204));
    // fix the leftmost nodes
    LinkedList<FemNode3d> leftNodes = getLeftNodes(femMod);
    LinkedList<FemNode3d> rightNodes = getRightNodes(femMod);
    myAttachNodes = rightNodes.toArray(new FemNode3d[0]);
    double wx, wy, wz;
    double mass;
    RigidTransform3d X = new RigidTransform3d();
    PolygonalMesh mesh;
    MechModel mechMod = new MechModel("mech");
    // mechMod.setPrintState ("%10.5f");
    wx = 0.1;
    wy = 0.3;
    wz = 0.3;
    RigidBody leftBody = new RigidBody("leftBody");
    mass = wx * wy * wz * myDensity;
    leftBody.setInertia(SpatialInertia.createBoxInertia(mass, wx, wy, wz));
    mesh = MeshFactory.createBox(wx, wy, wz);
    // mesh.setRenderMaterial (Material.createSpecial (Material.GRAY));
    leftBody.setMesh(mesh, /* fileName= */
    null);
    X.R.setIdentity();
    X.p.set(-0.35, 0, 0);
    leftBody.setPose(X);
    leftBody.setDynamic(true);
    mechMod.addRigidBody(leftBody);
    RenderProps.setPointStyle(mechMod, Renderer.PointStyle.SPHERE);
    RigidTransform3d TCW = new RigidTransform3d();
    RigidTransform3d TCA = new RigidTransform3d();
    TCA.p.set(0, 0, wz / 2);
    TCA.R.mulAxisAngle(1, 0, 0, Math.PI / 2);
    TCW.mul(X, TCA);
    RevoluteJoint joint = new RevoluteJoint(leftBody, TCA, TCW);
    RenderProps.setCylindricalLines(joint, 0.01, Color.BLUE);
    joint.setAxisLength(0.5);
    mechMod.addBodyConnector(joint);
    // right box
    RigidBody rightBody = new RigidBody("rightBody");
    rightBody.setInertia(SpatialInertia.createBoxInertia(mass, wx, wy, wz));
    mesh = MeshFactory.createBox(wx, wy, wz);
    // mesh.setRenderMaterial (Material.createSpecial (Material.GRAY));
    rightBody.setMesh(mesh, /* fileName= */
    null);
    X.p.set(0.35, 0, 0);
    rightBody.setPose(X);
    rightBody.setDynamic(true);
    mechMod.addRigidBody(rightBody);
    mechMod.addModel(femMod);
    for (FemNode3d n : leftNodes) {
        mechMod.attachPoint(n, leftBody);
    }
    // femMod.setProfile (true);
    mechMod.setIntegrator(Integrator.BackwardEuler);
    setConnected(true);
    addModel(mechMod);
    // mechMod.setProfiling (true);
    // int numWays = 10;
    // double res = 0.1;
    // for (int i = 0; i < numWays; i++) {
    // addWayPoint (new WayPoint (TimeBase.secondsToTicks ((i + 1) * res)));
    // }
    // addWayPoint (new WayPoint (TimeBase.secondsToTicks (5)));
    // addBreakPoint(5);
    // setupOutputProbes();
    addControlPanel(mechMod, femMod);
}