Examples with RotationMatrix3d maspack.matrix.RotationMatrix3d used on opensource projects

Example 11 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class CustomSphericalCoupling method setRpy.

public void setRpy(RigidTransform3d TGD, Vector3d angs) {
    TGD.setIdentity();
    RotationMatrix3d RDC = new RotationMatrix3d();
    RDC.setRpy(angs.z, angs.y, angs.x);
    TGD.R.transpose(RDC);
    checkConstraintStorage();
    myConstraintInfo[5].coordinate = angs.x;
    myConstraintInfo[4].coordinate = angs.y;
    myConstraintInfo[3].coordinate = angs.z;
}

Example 12 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class ParameterizedCoupling method getConstraintInfo.

@Override
public void getConstraintInfo(ConstraintInfo[] info, RigidTransform3d TGD, RigidTransform3d TCD, RigidTransform3d XERR, boolean setEngaged) {
    // projectToConstraint(TGD, TCD);
    // myXFC.mulInverseLeft(TGD, TCD);
    myErr.set(XERR);
    // set zeros
    switch(myLimitType) {
        case FIXED:
            setDistancesAndZeroDerivatives(info, 6, myErr);
            break;
        case ROLL:
            setDistancesAndZeroDerivatives(info, 5, myErr);
            break;
        case CURVE:
            setDistanceAndZeroDerivative(info[5], myErr);
        case ROLL_CURVE:
        case NONE:
            setDistancesAndZeroDerivatives(info, 3, myErr);
    }
    // roll limit
    if (myLimitType == LimitType.ROLL_CURVE || myLimitType == LimitType.ROLL) {
        RotationMatrix3d RDC = new RotationMatrix3d();
        RDC.transpose(TGD.R);
        double theta = doGetRoll(RDC);
        if (hasRestrictedRollRange()) {
            if (setEngaged) {
                maybeSetEngaged(info[5], theta, myMinRoll, myMaxRoll);
            }
            if (info[5].engaged != 0) {
                info[5].distance = getDistance(theta, myMinRoll, myMaxRoll);
                info[5].dotWrenchC.setZero();
                if (info[5].engaged == 1) {
                    info[5].wrenchC.set(0, 0, 0, 0, 0, 1);
                } else if (info[5].engaged == -1) {
                    info[5].wrenchC.set(0, 0, 0, 0, 0, -1);
                }
            }
        }
    }
    // curve limit
    if (myLimitType == LimitType.CURVE || myLimitType == LimitType.ROLL_CURVE) {
        // get relative z axis
        double[] z = new double[3];
        double[] axis = new double[3];
        double[] znew = new double[3];
        boolean engage = false;
        double theta = 0;
        TGD.R.getColumn(2, znew);
        TCD.R.getColumn(2, z);
        if (Math.abs(z[0] - znew[0]) > EPSILON) {
            System.out.printf("XFDz = (%f, %f, %f),  XCDz = (%f, %f, %f)\n", TCD.R.m02, TCD.R.m12, TCD.R.m22, TGD.R.m02, TGD.R.m12, TGD.R.m22);
        }
        theta = myCurve.getProjection(z, znew, axis);
        Vector3d v = new Vector3d(axis);
        v.normalize();
        v.inverseTransform(TGD);
        // check if z inside curve, and project to boundary if outside
        if (!myCurve.isWithin(z)) {
            if (theta > 1e-5) {
                engage = true;
                theta = -theta;
            } else {
                v.set(1, 0, 0);
            }
        }
        if (engage && Math.abs(theta) > Math.toRadians(5)) {
            System.out.println("Axis: " + v + ", Angle: " + Math.toDegrees(theta));
        }
        // maybe set engaged
        if (setEngaged) {
            if (engage) {
                info[4].engaged = 1;
            } else {
            // info[4].engaged = 0; // ?? is this required?
            }
        }
        if (info[4].engaged != 0) {
            info[4].distance = theta;
            info[4].wrenchC.set(0, 0, 0, -v.x, -v.y, -v.z);
            info[4].dotWrenchC.setZero();
        }
    }
}

Example 13 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class SpatialInertia method setRandom.

/**
 * Sets the components of this spatial inertia to uniformly distributed
 * random values in a specified range, using a supplied random number
 * generator. The components include the mass, center of mass, and rotational
 * inertia. The results are adjusted to ensure that the mass is positive and
 * the rotational inertia matrix is positive definite.
 *
 * @param lower
 * lower random value (inclusive)
 * @param upper
 * upper random value (exclusive)
 * @param generator
 * random number generator
 */
public void setRandom(double lower, double upper, Random generator) {
    double range = upper - lower;
    mass = 0;
    while (mass == 0) {
        mass = Math.abs(generator.nextDouble() * range + lower);
    }
    com.setRandom(lower, upper, generator);
    RotationMatrix3d U = new RotationMatrix3d();
    U.setRandom();
    // use L to form J
    L.setZero();
    for (int i = 0; i < 3; i++) {
        double elem = 0;
        while (elem == 0) {
            elem = Math.abs(generator.nextDouble() * range + lower);
        }
        L.set(i, i, elem);
    }
    L.mul(U, L);
    L.mulTranspose(U);
    J.set(L);
    updateMatrixElements();
}

Example 14 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class SphericalCoupling method getConstraintInfo.

@Override
public void getConstraintInfo(ConstraintInfo[] info, RigidTransform3d TGD, RigidTransform3d TCD, RigidTransform3d XERR, boolean setEngaged) {
    // projectToConstraint(TGD, TCD);
    // info[0].bilateral = true;
    // info[1].bilateral = true;
    // info[2].bilateral = true;
    // info[3].bilateral = false;
    // info[4].bilateral = false;
    // info[5].bilateral = false;
    // myXFC.mulInverseLeft(TGD, TCD);
    myErr.set(XERR);
    setDistancesAndZeroDerivatives(info, 3, myErr);
    if (myRangeType == TILT_LIMIT) {
        Vector3d utilt = new Vector3d();
        // Tilt axis is is z(C) x z(D).
        // In C coordinates, z(C) = (0,0,1) and z(D) = last row of TGD.R.
        // In D coordinates, z(D) = (0,0,1) and z(C) = last col of TGD.R.
        // D coordinates
        utilt.set(TGD.R.m12, -TGD.R.m02, 0);
        // utilt.set (-TGD.R.m21, TGD.R.m20, 0); // in C coordinates
        double ulen = utilt.norm();
        double theta = 0;
        if (ulen > 1e-8) {
            theta = Math.atan2(ulen, TGD.R.m22);
            utilt.scale(1 / ulen);
        }
        if (setEngaged) {
            if (theta > myMaxTilt) {
                info[3].engaged = 1;
            }
        }
        if (info[3].engaged != 0) {
            info[3].distance = myMaxTilt - theta;
            utilt.inverseTransform(TGD.R);
            info[3].wrenchC.set(0, 0, 0, utilt.x, utilt.y, utilt.z);
            info[3].dotWrenchC.setZero();
        }
    } else if (myRangeType == ROTATION_LIMIT) {
        Vector3d u = new Vector3d();
        double ang = TGD.R.getAxisAngle(u);
        // paranoid
        u.normalize();
        Vector3d a = new Vector3d(u.x * myMaxRotX, u.y * myMaxRotY, u.z * myMaxRotZ);
        double maxAng = a.norm();
        if (setEngaged) {
            if (ang > 0 && ang > maxAng) {
                info[3].engaged = 1;
            }
        }
        if (info[3].engaged != 0) {
            info[3].distance = maxAng - ang;
            u.x /= myMaxRotX;
            u.y /= myMaxRotY;
            u.z /= myMaxRotZ;
            u.normalize();
            info[3].wrenchC.set(0, 0, 0, -u.x, -u.y, -u.z);
            // TODO set this
            info[3].dotWrenchC.setZero();
        }
    } else if (myRangeType == RPY_LIMIT) {
        double roll, pitch, yaw;
        double[] rpy = new double[3];
        RotationMatrix3d RDC = new RotationMatrix3d();
        Vector3d wBA = new Vector3d();
        RDC.transpose(TGD.R);
        doGetRpy(rpy, RDC);
        roll = rpy[0];
        pitch = rpy[1];
        yaw = rpy[2];
        double cr = Math.cos(roll);
        double sr = Math.sin(roll);
        double cp = Math.cos(pitch);
        double sp = Math.sin(pitch);
        double denom = cp;
        // the singularity at cp = 0
        if (Math.abs(denom) < 0.0001) {
            denom = (denom >= 0 ? 0.0001 : -0.0001);
        }
        double tp = sp / denom;
        // Don't need to transform because vel is now in Frame C
        // get angular velocity of B with respect to A in frame C
        // if (!myComputeVelInFrameC) {
        // wBA.transform(RDC, myVelBA.w);
        // }
        info[3].distance = 0;
        info[4].distance = 0;
        info[5].distance = 0;
        double dotp = -sr * wBA.x + cr * wBA.y;
        double doty = (cr * wBA.x + sr * wBA.y) / denom;
        double dotr = wBA.z + sp * doty;
        if (setEngaged) {
            maybeSetEngaged(info[3], roll, myMinRoll, myMaxRoll);
            maybeSetEngaged(info[4], pitch, myMinPitch, myMaxPitch);
            maybeSetEngaged(info[5], yaw, myMinYaw, myMaxYaw);
        }
        if (info[3].engaged != 0) {
            info[3].distance = getDistance(roll, myMinRoll, myMaxRoll);
            info[3].wrenchC.set(0, 0, 0, sp * cr / denom, sp * sr / denom, 1);
            info[3].dotWrenchC.f.setZero();
            double tt = (1 + tp * tp);
            info[3].dotWrenchC.m.set(-sr * tp * dotr + tt * cr * dotp, cr * tp * dotr + tt * sr * dotp, 0);
            // checkDeriv ("roll", info[3], conR);
            if (info[3].engaged == -1) {
                info[3].wrenchC.negate();
                info[3].dotWrenchC.negate();
            }
        }
        if (info[4].engaged != 0) {
            info[4].distance = getDistance(pitch, myMinPitch, myMaxPitch);
            info[4].wrenchC.set(0, 0, 0, -sr, cr, 0);
            info[4].dotWrenchC.f.setZero();
            info[4].dotWrenchC.m.set(-cr * dotr, -sr * dotr, 0);
            // checkDeriv ("pitch", info[4], conP);
            if (info[4].engaged == -1) {
                info[4].wrenchC.negate();
                info[4].dotWrenchC.negate();
            }
        }
        if (info[5].engaged != 0) {
            info[5].distance = getDistance(yaw, myMinYaw, myMaxYaw);
            info[5].wrenchC.set(0, 0, 0, cr / denom, sr / denom, 0);
            info[5].dotWrenchC.f.setZero();
            info[5].dotWrenchC.m.set((-sr * dotr + cr * tp * dotp) / denom, (cr * dotr + sr * tp * dotp) / denom, 0);
            // checkDeriv ("yaw", info[5], conY);
            if (info[5].engaged == -1) {
                info[5].wrenchC.negate();
                info[5].dotWrenchC.negate();
            }
        }
    // System.out.println ("xxx:");
    // for (int i=3; i<6; i++) {
    // System.out.println (
    // info[i].engaged+" "+Math.toDegrees(info[i].distance));
    // }
    } else if (myRangeType != 0) {
        throw new InternalErrorException("Unimplemented range limits " + NumberFormat.formatHex(myRangeType));
    }
}

Example 15 with RotationMatrix3d

use of maspack.matrix.RotationMatrix3d in project artisynth_core by artisynth.

the class SphericalCoupling method setRpy.

public void setRpy(RigidTransform3d TGD, Vector3d angs) {
    TGD.setIdentity();
    RotationMatrix3d RDC = new RotationMatrix3d();
    RDC.setRpy(angs.z, angs.y, angs.x);
    TGD.R.transpose(RDC);
    checkConstraintStorage();
    myConstraintInfo[5].coordinate = angs.x;
    myConstraintInfo[4].coordinate = angs.y;
    myConstraintInfo[3].coordinate = angs.z;
}