/** * Sets the value of this matrix to the transpose of the argument matrix. * @param m1 the matrix to be transposed */ public final void transpose(Matrix3d m1) { if (this != m1) { this.m00 = m1.m00; this.m01 = m1.m10; this.m02 = m1.m20; this.m10 = m1.m01; this.m11 = m1.m11; this.m12 = m1.m21; this.m20 = m1.m02; this.m21 = m1.m12; this.m22 = m1.m22; } else this.transpose(); }
public Matrix3d getMTranspose() { if (Mtransp!=null){ return Mtransp; } Matrix3d M = getM(); Mtransp = new Matrix3d(); Mtransp.transpose(M); return Mtransp; }
public DigitalIMUSensorProcessor(RawIMUSensorsInterface rawIMUSensors, ProcessedIMUSensorsWriteOnlyInterface processedIMUSensors, int imuIndex, double localGravityPositiveZ) { this.rawIMUSensors = rawIMUSensors; this.processedSensors = processedIMUSensors; this.imuIndex = imuIndex; this.localGravityZ = localGravityPositiveZ; IMUCalibrationProperties imuCalibrationProperties = new IMUCalibrationProperties(imuIndex); this.orientationOffset = imuCalibrationProperties.getOrientationOffset(); this.orientationOffsetTranspose.transpose(this.orientationOffset); this.accelerationOffset = imuCalibrationProperties.getAccelerationOffset(); imuCalibrationProperties.save(); }
/** * @return a 6 x 6 matrix representing this generalized inertia in matrix form [massMomentOfInertia, crossTranspose; cross, mI] */ public void getMatrix(DenseMatrix64F matrixToPack) { // upper left block MatrixTools.setDenseMatrixFromMatrix3d(0, 0, massMomentOfInertiaPart, matrixToPack); // lower left MatrixTools.toTildeForm(tempForCrossPartTilde, crossPart); MatrixTools.setDenseMatrixFromMatrix3d(3, 0, tempForCrossPartTilde, matrixToPack); // upper right block tempForCrossPartTilde.transpose(); MatrixTools.setDenseMatrixFromMatrix3d(0, 3, tempForCrossPartTilde, matrixToPack); // lower right block for (int i = 3; i < 6; i++) { matrixToPack.set(i, i, mass); } }
orientationMeasurementTransposed.transpose(orientationMeasurement);
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); }
rotationMatrix.transpose(); polygonToPack.clear(); for (Point3d point : projectedPoints)
rotationMatrix.transpose();
/** * Changes the frame in which this GeneralizedRigidBodyInertia is expressed. * * See Duindam, Port-Based Modeling and Control for Efficient Bipedal Walking Robots, page 40, eq. (2.57) * http://sites.google.com/site/vincentduindam/publications * * @param newFrame the frame in which this inertia should be expressed */ public void changeFrame(ReferenceFrame newFrame) { RigidBodyTransform transform = newFrame.getTransformToDesiredFrame(expressedInframe); Matrix3d rotation = new Matrix3d(); transform.getRotation(rotation); transform.getTranslation(tempVector); // p // mass moment of inertia part: subTildeTimesTildePlusTildeTimesTildeTransposeFromSymmetricMatrix(massMomentOfInertiaPart, crossPart, tempVector); // J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) subScalarTimesTildeTimesTildeFromSymmetricMatrix(massMomentOfInertiaPart, tempVector, mass); // J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) - m * tilde(p) * tilde(p) massMomentOfInertiaPart.mulTransposeLeft(rotation, massMomentOfInertiaPart); // RTranspose * (J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) - m * tilde(p) * tilde(p)) massMomentOfInertiaPart.mul(rotation); // RTranspose * (J - (tilde(c) * tilde(p))^T - tilde(c) * tilde(p) - m * tilde(p) * tilde(p)) * R: done. // cross part: tempVector.scale(mass); // m * p crossPart.add(tempVector); // c + m * p rotation.transpose(); // RTranspose rotation.transform(crossPart); // RTranspose * (c + m * p) // mass part doesn't change // finally, change frame and check if cross term zero this.expressedInframe = newFrame; determineIfCrossPartIsZero(); }
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(); }
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 getTransformedFeedbackGains(FrameVector2d feedbackGainsToPack) { double epsilonZeroICPVelocity = 1e-5; if (desiredICPVelocity.lengthSquared() > MathTools.square(epsilonZeroICPVelocity)) { icpVelocityDirectionFrame.setXAxis(desiredICPVelocity); RigidBodyTransform transform = icpVelocityDirectionFrame.getTransformToWorldFrame(); transform.getRotation(rotation); rotationTranspose.set(rotation); rotationTranspose.transpose(); transformTranspose.setRotation(rotationTranspose); gainsMatrix.setZero(); gainsMatrix.setElement(0, 0, 1.0 + feedbackParallelGain.getDoubleValue()); gainsMatrix.setElement(1, 1, 1.0 + feedbackOrthogonalGain.getDoubleValue()); gainsMatrixTransformed.set(rotation); gainsMatrixTransformed.mul(gainsMatrix); gainsMatrixTransformed.mul(rotationTranspose); feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.setX(gainsMatrixTransformed.getElement(0, 0)); feedbackGainsToPack.setY(gainsMatrixTransformed.getElement(1, 1)); } else { feedbackGainsToPack.setToZero(worldFrame); feedbackGainsToPack.set(feedbackOrthogonalGain.getDoubleValue(), feedbackOrthogonalGain.getDoubleValue()); } yoFeedbackGains.set(feedbackGainsToPack); }
rotationMatrix.transpose(); rotationMatrix.transform(translation); RigidBodyTransform translationTransform = new RigidBodyTransform();
Matrix3d corr = tempCorrMatrix3d.get(); x.transpose(rotationMatrixToOrthogonalize);