public void setHeadingAngleAndUpdate(double headingAngle) { rotation.rotZ(headingAngle); update(); } }
@Override public Point3d[] getVertices() { Point3d[] icosahedron = new Point3d[12]; // see http://answers.yahoo.com/question/index?qid=20080108041441AAJCjEu double c = circumscribedRadius * 1 / Math.sqrt(5); double s = 2 * c; // golden ratio double c1 = Math.sqrt((3-Math.sqrt(5))/8); // cos(2Pi/5) double s1 = Math.sqrt((5+Math.sqrt(5))/8); // sin(2Pi/5) double c2 = Math.sqrt((3+Math.sqrt(5))/8); // cos(Pi/5) double s2 = Math.sqrt((5-Math.sqrt(5))/8); // sin(Pi/5) icosahedron[0] = new Point3d(0, 0, circumscribedRadius); icosahedron[1] = new Point3d(s, 0, c); icosahedron[2] = new Point3d(s*c1, s*s1, c); icosahedron[3] = new Point3d(-s*c2, s*s2, c); icosahedron[4] = new Point3d(-s*c2, -s*s2, c); icosahedron[5] = new Point3d(s*c1, -s*s1, c); for (int i = 0; i < 6; i++) { icosahedron[i+6] = new Point3d(icosahedron[i]); icosahedron[i+6].negate(); } Matrix3d m = new Matrix3d(); m.rotZ(Math.PI/10); for (Point3d p: icosahedron) { m.transform(p); } return icosahedron; };
/** * Returns the vertices of an n-fold polygon of given radius and center * @return */ public static Point3d[] getPolygonVertices(int n, double radius, Point3d center) { Point3d[] polygon = new Point3d[n]; Matrix3d m = new Matrix3d(); for (int i = 0; i < n; i++) { polygon[i] = new Point3d(0, radius, 0); m.rotZ(i*2*Math.PI/n); m.transform(polygon[i]); polygon[i].add(center); } return polygon; }
pinJointZRotation.rotZ(-FastMath.PI / 2.0); Matrix3d pinJointRotation = new Matrix3d(); pinJointRotation.rotY(-FastMath.PI / 4.0);
/** * Rotates the coordinate system counter clockwise around the specified axis by the given * angle in radians. This does not rotate existing graphics, instead it rotates a "cursor" * when another object is added it will be centered on the origin of the current system * as described by the translations and rotations applied since its creation at the joint * origin. * * @param rotationAngle the angle to rotate around the specified axis in radians. * @param rotationAxis Axis around which to rotate. Either Link.X, Link.Y or Link.Z */ public void rotate(double rotationAngle, Axis rotationAxis) { Matrix3d rot = new Matrix3d(); if (rotationAxis == Axis.X) rot.rotX(rotationAngle); else if (rotationAxis == Axis.Y) rot.rotY(rotationAngle); else if (rotationAxis == Axis.Z) rot.rotZ(rotationAngle); rotate(rot); }
/** * Rotates the coordinate system counter clockwise around the specified axis by the given * angle in radians. This does not rotate existing graphics, instead it rotates a "cursor" * when another object is added it will be centered on the origin of the current system * as described by the translations and rotations applied since its creation at the joint * origin. * * @param rotationAngle the angle to rotate around the specified axis in radians. * @param rotationAxis Axis around which to rotate. Either Link.X, Link.Y or Link.Z */ public void rotate(double rotationAngle, Axis rotationAxis) { Matrix3d rot = new Matrix3d(); if (rotationAxis == Axis.X) rot.rotX(rotationAngle); else if (rotationAxis == Axis.Y) rot.rotY(rotationAngle); else if (rotationAxis == Axis.Z) rot.rotZ(rotationAngle); rotate(rot); }
private void createButton(String buttonRobotName, Point3d buttonLocation, Vector3d pushVector, double forceVectorScale) { Matrix3d zRotation = new Matrix3d(); Matrix3d yRotation = new Matrix3d(); Matrix3d rotation = new Matrix3d(); pushVector.normalize(); double alpha = Math.atan(pushVector.getY() / pushVector.getX()); double beta = (Math.PI / 2.0) - Math.atan(pushVector.getZ() / Math.sqrt(Math.pow(pushVector.getX(), 2.0) + Math.pow(pushVector.getY(),2.0))); zRotation.rotZ(alpha); yRotation.rotY(beta); rotation.mul(zRotation, yRotation); RigidBodyTransform rootBodyTransform = new RigidBodyTransform(rotation, new Vector3d(buttonLocation)); ContactableButtonRobot button = new ContactableButtonRobot(buttonRobotName, rootBodyTransform, pushVector); button.createButtonRobot(); button.createAvailableContactPoints(1, NUMBER_OF_CONTACT_POINTS, forceVectorScale, true); buttonRobots.add(button); }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // front vertex-centered break; case 1: m.rotX(-0.6523581397843639); // back face-centered -0.5535743588970415 m.rotX(Math.toRadians(-26)); break; case 2: m.rotZ(Math.PI/2); Matrix3d m1 = new Matrix3d(); m1.rotX(-1.0172219678978445); m.mul(m1); break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
@Override public Matrix3d getViewMatrix(int index) { Matrix3d m = new Matrix3d(); switch (index) { case 0: m.setIdentity(); // C4 vertex-centered break; case 1: m.rotX(-0.5 * TETRAHEDRAL_ANGLE); // C3 face-centered 2.0*Math.PI/3 Matrix3d m1 = new Matrix3d(); m1.rotZ(Math.PI/4); m.mul(m1); break; case 2: m.rotY(Math.PI/4); // side face-centered break; default: throw new IllegalArgumentException("getViewMatrix: index out of range:" + index); } return m; }
/** * Returns the vertices of an n-fold polygon of given radius and center * @return */ @Override public Point3d[] getVertices() { Point3d[] polygon = new Point3d[2*n]; Matrix3d m = new Matrix3d(); Point3d center = new Point3d(0, 0, height/2); for (int i = 0; i < n; i++) { polygon[i] = new Point3d(0, circumscribedRadius, 0); m.rotZ(i*2*Math.PI/n); m.transform(polygon[i]); polygon[n+i] = new Point3d(polygon[i]); polygon[i].sub(center); polygon[n+i].add(center); } return polygon; };
public void corrupt(Matrix3d signal) { yawDriftVelocity.add(yawDriftAcceleration.getDoubleValue() * dt); yawDriftAngle.add(yawDriftVelocity.getDoubleValue() * dt); yawDriftRotation.rotZ(yawDriftAngle.getDoubleValue()); tempRotation.mul(yawDriftRotation, signal); signal.set(tempRotation); corruptedIMUYawAngle.set(RotationTools.computeYaw(tempRotation)); } }
private void calculateFramedMOITensor() { framedMOI = RotationMatrices.getZeroMatrix(3); Matrix3d pitch = new Matrix3d(); Matrix3d yaw = new Matrix3d(); Matrix3d roll = new Matrix3d(); pitch.rotX(Math.toRadians(parent.wrapper.pitch)); yaw.rotY(Math.toRadians(parent.wrapper.yaw)); roll.rotZ(Math.toRadians(parent.wrapper.roll)); pitch.mul(yaw); pitch.mul(roll); pitch.normalize(); Matrix3d inertiaBodyFrame = new Matrix3d(MoITensor); Matrix3d multipled = new Matrix3d(); multipled.mul(pitch, inertiaBodyFrame); pitch.transpose(); multipled.mul(pitch); framedMOI[0] = multipled.m00; framedMOI[1] = multipled.m01; framedMOI[2] = multipled.m02; framedMOI[3] = multipled.m10; framedMOI[4] = multipled.m11; framedMOI[5] = multipled.m12; framedMOI[6] = multipled.m20; framedMOI[7] = multipled.m21; framedMOI[8] = multipled.m22; invFramedMOI = RotationMatrices.inverse3by3(framedMOI); }
public FramePose getPoseFromCylindricalCoordinates(RobotSide robotSide, ReferenceFrame frame, double radiansFromYAxis, double radius, double z, double outwardRotation, double pitchRotation) { getPosition(position, frame, radiansFromYAxis, radius, z); RotationTools.convertYawPitchRollToMatrix(0.0, Math.PI / 2.0, -Math.PI / 2.0, preRotation); rotX.rotX(robotSide.negateIfRightSide(Math.PI / 2.0) - radiansFromYAxis); rotZ.rotZ(robotSide.negateIfRightSide(outwardRotation)); rotY.rotY(pitchRotation); rotation.mul(preRotation, rotX); rotation.mul(rotZ); rotation.mul(rotY); orientation.setIncludingFrame(frame, rotation); return new FramePose(position, orientation); }
cinderBlockOrientation.rotZ(Math.toRadians(courseAngle)); cinderBlockOrientation.mul(pitchRollMatrix);
yawRotation.rotZ(amountToYaw);
private void updateRootJointRotation() { // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); rotationFromRootJointFrameToWorld.mul(rotationFrozenOffset, rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); rootJointFrame.update(); if (imuYawDriftEstimator != null) { imuYawDriftEstimator.update(); yawBiasMatrix.rotZ(imuYawDriftEstimator.getYawBiasInWorldFrame()); yawBiasMatrix.transpose(); rotationFromRootJointFrameToWorld.mul(yawBiasMatrix, rotationFromRootJointFrameToWorld); } rootJoint.setRotation(rotationFromRootJointFrameToWorld); rootJointFrame.update(); }
@Override public void initializeForFrozenState() { rotationFrozenOffset.setIdentity(); // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); double initialYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); rootJointYawOffsetFromFrozenState.set(initialYaw); rotationFrozenOffset.rotZ(initialYaw); yoRootJointFrameQuaternion.setToZero(); yoRootJointFrameOrientation.setToZero(); yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
@Override public void updateForFrozenState() { // R_{measurementFrame}^{world} imuProcessedOutput.getOrientationMeasurement(orientationMeasurement); transformFromMeasurementFrameToWorld.setRotationAndZeroTranslation(orientationMeasurement); // R_{root}^{measurementFrame} rootJointFrame.getTransformToDesiredFrame(transformFromRootJointFrameToMeasurementFrame, measurementFrame); // R_{root}^{world} = R_{estimationLink}^{world} * R_{root}^{estimationLink} transformFromRootJointFrameToWorld.multiply(transformFromMeasurementFrameToWorld, transformFromRootJointFrameToMeasurementFrame); transformFromRootJointFrameToWorld.getRotation(rotationFromRootJointFrameToWorld); yoRootJointFrameQuaternion.getYawPitchRoll(lastComputedYawPitchRoll); double currentYaw = RotationTools.computeYaw(rotationFromRootJointFrameToWorld); double yawDifference = AngleTools.computeAngleDifferenceMinusPiToPi(lastComputedYawPitchRoll[0], currentYaw); rootJointYawOffsetFromFrozenState.set(yawDifference); rotationFrozenOffset.rotZ(yawDifference); // Keep setting the orientation so that the localization updater works properly. yoRootJointFrameQuaternion.get(rotationFromRootJointFrameToWorld); rootJoint.setRotation(rotationFromRootJointFrameToWorld); // Set the rootJoint twist to zero. rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); updateViz(); }