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

Example 46 with RigidBody

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

Example 47 with RigidBody

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

the class FemMuscleArm method getCollisions.

public boolean getCollisions() {
    MechModel mech = (MechModel) models().get(0);
    PointToPointMuscle muscle = (PointToPointMuscle) mech.findComponent("models/0");
    RigidBody upperArm = (RigidBody) mech.findComponent("rigidBodies/upper");
    if (muscle == null || upperArm == null) {
        return false;
    }
    CollisionBehavior cb = mech.getCollisionBehavior(muscle, upperArm);
    return cb != null && cb.isEnabled();
}

Example 48 with RigidBody

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

the class FemMuscleArm method addFemMuscle.

public void addFemMuscle(Point3d upper, Point3d lower) throws IOException {
    upperArm = model.rigidBodies().get("upper");
    RigidBody lowerArm = model.rigidBodies().get("lower");
    if (upperArm == null || lowerArm == null) {
        return;
    }
    muscle = new PointToPointMuscle("muscle", 1.0, 0.85, "muscle", true);
    model.addModel(muscle);
    RigidTransform3d X = new RigidTransform3d();
    // Point3d upper = new Point3d(2.4, 0, 20), lower = new Point3d(3, 0, -7);
    FemNode3d first = muscle.getFirstNode(), last = muscle.getLastNode();
    double scale = (upper.distance(lower) / first.getPosition().distance(last.getPosition()));
    System.out.println(scale);
    AffineTransform3d scaling = new AffineTransform3d();
    scaling.setIdentity();
    scaling.applyScaling(scale, scale, scale);
    // muscle.transformGeometry(scaling);
    Vector3d translate = new Vector3d();
    translate.sub(upper, first.getPosition());
    muscle.transformGeometry(new RigidTransform3d(translate, new AxisAngle()));
    Vector3d offfem = new Vector3d(), offtarget = new Vector3d();
    offfem.sub(last.getPosition(), first.getPosition());
    offtarget.sub(lower, upper);
    double angle = offfem.angle(offtarget);
    offfem.cross(offtarget);
    muscle.transformGeometry(new RigidTransform3d(new Vector3d(-upper.x, -upper.y, -upper.z), new AxisAngle()));
    muscle.transformGeometry(new RigidTransform3d(upper, new AxisAngle(offfem, angle)));
    muscle.getFirstNode().setDynamic(false);
    // ant.getFirstNode().setPosition(new Point3d(2.381, 0.000, 20));
    // model.AttachPoint(ant.getFirstNode(),upperArm,new Point3d(-size.x,0,-(size.z/2.0)/1.2));
    model.attachPoint(muscle.getLastNode(), lowerArm);
    muscle.setMaxForce(300000);
    muscle.setExcitation(0.0);
    RenderProps rp = new RenderProps(model.getRenderProps());
    rp.setFaceColor(Color.RED);
    rp.setLineStyle(Renderer.LineStyle.LINE);
    rp.setPointStyle(Renderer.PointStyle.POINT);
    rp.setShading(Renderer.Shading.SMOOTH);
    rp.setLineColor(Color.WHITE);
    muscle.setRenderProps(rp);
    muscle.setSurfaceRendering(SurfaceRender.Shaded);
}

Example 49 with RigidBody

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

the class ArticulatedDemo method createLongLinkage.

void createLongLinkage(MechModel mech, Point3d start, Point3d end, double curveRadius, double linkRadius, int nlinks) {
    RigidBody link = ground;
    Point3d lastTip = start;
    double chordLength = start.distance(end);
    double theta = 2 * Math.asin(chordLength / (2 * curveRadius));
    double chordHeight = curveRadius * Math.cos(theta / 2);
    Vector3d xdir = new Vector3d();
    Vector3d ydir = new Vector3d();
    xdir.sub(end, start);
    xdir.scale(1 / chordLength);
    xdir.normalize();
    ydir.scaledAdd(xdir.z, xdir, new Vector3d(0, 0, -1));
    ydir.normalize();
    for (int i = 0; i < nlinks - 1; i++) {
        double ang = theta / 2 - (i + 1) * theta / nlinks;
        double x = chordLength / 2 - curveRadius * Math.sin(ang);
        double y = curveRadius * Math.cos(ang) - chordHeight;
        Point3d tip = new Point3d();
        tip.set(start);
        tip.scaledAdd(x, xdir, tip);
        tip.scaledAdd(y, ydir, tip);
        link = addSphericalLinkage(mech, linkRadius, link, lastTip, null, tip);
        lastTip = tip;
    }
    addSphericalLinkage(mech, linkRadius, link, lastTip, null, end);
}

Example 50 with RigidBody

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

the class ArticulatedDemo method createLadderPlaneCollider.

void createLadderPlaneCollider(MechModel mech) {
    usePlanarContacts = true;
    Point3d base0 = new Point3d(-5, 2, 9);
    Point3d tip0 = new Point3d(0, 2, 5);
    Point3d tip1 = new Point3d(5, 2, 3);
    Point3d tip2 = new Point3d(10, 2, 3);
    double radius = 1.0;
    RigidBody link0 = addSphericalLinkage(mech, radius, ground, base0, null, tip0);
    RigidBody link1 = addSphericalLinkage(mech, radius, link0, tip0, null, tip1);
    RigidBody link2 = addSphericalLinkage(mech, radius, link1, tip1, null, tip2);
    Point3d base1 = new Point3d(-5, -2, 9);
    Point3d tip3 = new Point3d(0, -2, 5);
    Point3d tip4 = new Point3d(5, -2, 3);
    Point3d tip5 = new Point3d(10, -2, 3);
    RigidBody link3 = addSphericalLinkage(mech, radius, ground, base1, null, tip3);
    RigidBody link4 = addSphericalLinkage(mech, radius, link3, tip3, null, tip4);
    RigidBody link5 = addSphericalLinkage(mech, radius, link4, tip4, null, tip5);
    addSphericalLinkage(mech, radius, link5, tip5, link2, tip2);
    addSphericalLinkage(mech, radius, link4, tip4, link1, tip1);
}