@Override public void setToNaN() { super.setIdentity(); super.mul(Double.NaN); }
public static Matrix3d rotate(Matrix3d inertialFrameRotation, Matrix3d inertia) { Matrix3d temp = new Matrix3d(); temp.mulTransposeRight(inertia, inertialFrameRotation); Matrix3d result = new Matrix3d(); result.mul(inertialFrameRotation, temp); return result; // // inertialFrameRotation.transpose(); // inertia.mul(inertialFrameRotation); // inertialFrameRotation.transpose(); // inertialFrameRotation.mul(inertia); }
private void processOrientation() { rawIMUSensors.getOrientation(rotationMatrixBeforeOffset, imuIndex); rotationMatrix.mul(rotationMatrixBeforeOffset, orientationOffset); processedSensors.setRotation(rotationMatrix, imuIndex); }
public void corrupt(Matrix3d signal) { generateGaussianRotation(noiseAxisAngle, random, standardDeviation.getDoubleValue()); noiseRotationMatrix.set(noiseAxisAngle); signal.mul(noiseRotationMatrix); }
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); }
/** * Transform given Matrix4d in orthonormal basis to the crystal basis using * the PDB axes convention (NCODE=1) * @param m * @return */ public Matrix4d transfToCrystal(Matrix4d m) { Vector3d trans = new Vector3d(m.m03,m.m13,m.m23); transfToCrystal(trans); Matrix3d rot = new Matrix3d(); m.getRotationScale(rot); // see Giacovazzo section 2.E, eq. 2.E.1 (the inverse equation) // R = MT * Rprime * MT-1 rot.mul(this.getMTransposeInv()); rot.mul(this.getMTranspose(),rot); return new Matrix4d(rot,trans,1.0); }
/** * Transform given Matrix4d in crystal basis to the orthonormal basis using * the PDB axes convention (NCODE=1) * @param m * @return */ public Matrix4d transfToOrthonormal(Matrix4d m) { Vector3d trans = new Vector3d(m.m03,m.m13,m.m23); transfToOrthonormal(trans); Matrix3d rot = new Matrix3d(); m.getRotationScale(rot); // see Giacovazzo section 2.E, eq. 2.E.1 // Rprime = MT-1 * R * MT rot.mul(this.getMTranspose()); rot.mul(this.getMTransposeInv(),rot); return new Matrix4d(rot,trans,1.0); }
private void integrateError() { double yawNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_yaw.getDoubleValue() + noiseBias_yaw.getDoubleValue(); double pitchNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_pitch.getDoubleValue() + noiseBias_pitch.getDoubleValue(); double rollNoise = (random.nextDouble() - 0.5) * Math.PI * noiseScalar_roll.getDoubleValue() + noiseBias_roll.getDoubleValue(); RotationTools.convertYawPitchRollToMatrix(yawNoise, pitchNoise, rollNoise, rotationNoise); rotationError.mul(rotationNoise); double xNoise = (random.nextDouble() - 0.5) * noiseScalar_x.getDoubleValue() + noiseBias_x.getDoubleValue(); double yNoise = (random.nextDouble() - 0.5) * noiseScalar_y.getDoubleValue() + noiseBias_y.getDoubleValue(); double zNoise = (random.nextDouble() - 0.5) * noiseScalar_z.getDoubleValue() + noiseBias_z.getDoubleValue(); translationNoise.set(xNoise, yNoise, zNoise); pelvisPose.getRotation(pelvisRotation); pelvisRotation.mul(rotationError); pelvisRotation.transform(translationNoise); translationError.add(translationNoise); } }
@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; }
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); }
public void update(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22) { perfectRotationMatrix.setM00(m00); perfectRotationMatrix.setM01(m01); perfectRotationMatrix.setM02(m02); perfectRotationMatrix.setM10(m10); perfectRotationMatrix.setM11(m11); perfectRotationMatrix.setM12(m12); perfectRotationMatrix.setM20(m20); perfectRotationMatrix.setM21(m21); perfectRotationMatrix.setM22(m22); generateNoise(); generateBias(); noisyRotationMatrix.mul(perfectRotationMatrix, biasMatrix); // NoiseMatrix is transformation from IMU measurement to perfect body orientation noisyRotationMatrix.mul(noisyRotationMatrix, noiseMatrix); // NoiseMatrix is transformation from IMU measurement to perfect body orientation if (((Double)(noisyRotationMatrix.getM00())).isNaN()) { noisyRotationMatrix.set(perfectRotationMatrix); } }
public void initializeOrientationEstimateToMeasurement() { // R^W_M OrientationSensorConfiguration firstOrientationSensorConfiguration = orientationSensorConfigurations.get(0); ControlFlowOutputPort<Matrix3d> firstOrientationMeasurementOutputPort = firstOrientationSensorConfiguration.getOutputPort(); Matrix3d measurementToWorld = firstOrientationMeasurementOutputPort.getData(); // R^M_E ReferenceFrame measurementFrame = firstOrientationSensorConfiguration.getMeasurementFrame(); FrameOrientation estimationFrameOrientation = new FrameOrientation(estimationFrame); estimationFrameOrientation.changeFrame(measurementFrame); Matrix3d estimationToMeasurement = estimationFrameOrientation.getMatrix3dCopy(); // R^W_E Matrix3d estimationToWorld = new Matrix3d(); estimationToWorld.mul(measurementToWorld, estimationToMeasurement); FrameOrientation initialOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), estimationToWorld); setEstimatedOrientation(initialOrientation); }
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 getTransformedWeights(FrameVector2d weightsToPack, double forwardWeight, double lateralWeight) { RigidBodyTransform transform = contactableFeet.get(supportSide.getEnumValue()).getSoleFrame().getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotation.transpose(); transformTranspose.setRotation(rotationTranspose); weightsMatrix.setZero(); weightsMatrix.setElement(0, 0, forwardWeight); weightsMatrix.setElement(1, 1, lateralWeight); weightsMatrixTransformed.set(rotation); weightsMatrixTransformed.mul(weightsMatrix); weightsMatrixTransformed.mul(rotationTranspose); weightsToPack.setToZero(worldFrame); weightsToPack.setX(weightsMatrixTransformed.getElement(0, 0)); weightsToPack.setY(weightsMatrixTransformed.getElement(1, 1)); }
private void computeOrientationStateOutputBlock(Matrix3d rotationFromPelvisToWorld) { computeVelocityOfStationaryPoint(tempFrameVector); tempFrameVector.changeFrame(estimationFrame); tempCenterOfMassVelocity.setIncludingFrame(centerOfMassVelocityPort.getData()); tempCenterOfMassVelocity.changeFrame(estimationFrame); tempFrameVector.sub(tempCenterOfMassVelocity); tempFrameVector.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFrameVector.getVector()); tempMatrix.mul(rotationFromPelvisToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort)); }
public void addNoise() { rootJointFrame.getTransformToParent(pelvisPose); updateBeforeYoVariables(); integrateError(); pelvisPose.getRotation(pelvisRotation); pelvisRotation.mul(rotationError); pelvisPose.setRotation(pelvisRotation); pelvisPose.getTranslation(pelvisTranslation); pelvisTranslation.add(translationError); pelvisPose.setTranslation(pelvisTranslation); updateAfterYoVariables(); rootJoint.setPositionAndRotation(pelvisPose); rootJointFrame.update(); }
private void applyCorrection(FramePoint desiredPosition, FrameOrientation desiredOrientation) { ReferenceFrame originalFrame = desiredPosition.getReferenceFrame(); desiredPosition.changeFrame(controlFrame); desiredOrientation.changeFrame(controlFrame); desiredPosition.sub(totalLinearCorrection); totalAngularCorrection.negate(); RotationTools.convertRotationVectorToAxisAngle(totalAngularCorrection.getVector(), angularDisplacementAsAxisAngle); angularDisplacementAsMatrix.set(angularDisplacementAsAxisAngle); desiredOrientation.getMatrix3d(correctedRotationMatrix); correctedRotationMatrix.mul(angularDisplacementAsMatrix, correctedRotationMatrix); desiredOrientation.set(correctedRotationMatrix); desiredPosition.changeFrame(originalFrame); desiredOrientation.changeFrame(originalFrame); }
private void computeAngularVelocityStateOutputBlock(Matrix3d rotationFromPelvisToWorld) { tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData()); tempCenterOfMassPosition.changeFrame(estimationFrame); ReferenceFrame referenceFrame = referenceFrameNameMap.getFrameByName(pointVelocityMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, pointVelocityMeasurementInputPort.getData().getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(estimationFrame); tempFramePoint.sub(tempCenterOfMassPosition); tempFramePoint.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint()); tempMatrix.mul(rotationFromPelvisToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(angularVelocityPort)); }
private void computeAngularAccelerationBlock(Matrix3d rotationFromEstimationToMeasurement) { // r tempFramePoint.setIncludingFrame(centerOfMassPositionPort.getData()); tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); tempFrameVector.setIncludingFrame(tempFramePoint); // r - p_{i}^{w} tempFramePoint.setToZero(measurementFrame); tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); tempFrameVector.sub(tempFramePoint); // R_{w}^{p} (r - p_{i}^{w}) tempFrameVector.changeFrame(estimationFrame); // \tilde{r^{p} - p_{i}^{p}} MatrixTools.toTildeForm(tempMatrix, tempFrameVector.getVector()); // R_{p}^{i} \tilde{r^{p} - p_{i}^{p}} tempMatrix.mul(rotationFromEstimationToMeasurement, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(angularAccelerationPort)); }
private void computeOrientationStateOutputBlock() { estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame()); tempTransform.getRotation(rotationFromEstimationToWorld); tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData()); tempCenterOfMassPosition.changeFrame(estimationFrame); ReferenceFrame referenceFrame = referenceFrameMap.getFrameByName(pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(estimationFrame); tempFramePoint.sub(tempCenterOfMassPosition); tempFramePoint.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint()); tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort)); }