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

Example 6 with PlanarConnector

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