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

Example 6 with SphericalJoint

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

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

the class ArticulatedDemo method addSphericalLinkage.

private RigidBody addSphericalLinkage(MechModel mechMod, double radius, RigidBody body0, Point3d pnt0, RigidBody body1, Point3d pnt1) {
    // set up the position of the link
    Vector3d u = new Vector3d();
    u.sub(pnt1, pnt0);
    double len = u.norm();
    u.scale(1 / len);
    RigidTransform3d XLinkToWorld = new RigidTransform3d();
    XLinkToWorld.p.scaledAdd(0.5 * len, u, pnt0);
    XLinkToWorld.R.setZDirection(u);
    // create the link
    PolygonalMesh mesh = MeshFactory.createRoundedCylinder(radius, len, /*nslices=*/
    12, /*nsegs=*/
    1, /*flatBottom=*/
    false);
    SpatialInertia inertia = SpatialInertia.createBoxInertia(/* mass= */
    10, radius, radius, len);
    RigidBody link = new RigidBody(null);
    link.setInertia(inertia);
    link.setMesh(mesh, null);
    link.setPose(XLinkToWorld);
    mechMod.addRigidBody(link);
    // create the spherical joint(s)
    RigidTransform3d TCW = new RigidTransform3d();
    RigidTransform3d TCA = new RigidTransform3d();
    if (body0 != null) {
        TCA.setIdentity();
        TCA.p.set(0, 0, -len / 2);
        TCW.mul(XLinkToWorld, TCA);
        SphericalJoint joint0;
        if (body0 == ground) {
            joint0 = new SphericalJoint(link, TCA, TCW);
        } else {
            RigidTransform3d TCB = new RigidTransform3d();
            TCB.mulInverseLeft(body0.getPose(), TCW);
            joint0 = new SphericalJoint(link, TCA, body0, TCB);
        }
        mechMod.addBodyConnector(joint0);
    }
    if (body1 != null) {
        TCA.setIdentity();
        TCA.p.set(0, 0, len / 2);
        TCW.mul(XLinkToWorld, TCA);
        SphericalJoint joint1;
        if (body1 == ground) {
            joint1 = new SphericalJoint(link, TCA, TCW);
        } else {
            RigidTransform3d TCB = new RigidTransform3d();
            TCB.mulInverseLeft(body1.getPose(), TCW);
            joint1 = new SphericalJoint(link, TCA, body1, TCB);
        }
        mechMod.addBodyConnector(joint1);
    }
    if (usePlanarContacts) {
        // set up a unilateral constraint at the tip
        TCW.setIdentity();
        TCW.p.set(0, 0, zPlane);
        Point3d pCA = new Point3d(0, 0, len / 2);
        PlanarConnector contact = new PlanarConnector(link, pCA, TCW);
        contact.setUnilateral(true);
        contact.setPlaneSize(20);
        mechMod.addBodyConnector(contact);
    }
    return link;
}