/** * Set diagonal elements of matrix to diagValues * * @param matrix * @param diagValues */ public static void setMatrixDiag(Matrix3d matrix, double[] diagValues) { for (int i = 0; i < 3; i++) { matrix.setElement(i, i, diagValues[i]); } }
public static void multOuter(Matrix3d result, Vector3d vector) { double x = vector.getX(); double y = vector.getY(); double z = vector.getZ(); result.setElement(0, 0, x * x); result.setElement(1, 1, y * y); result.setElement(2, 2, z * z); double xy = x * y; result.setElement(0, 1, xy); result.setElement(1, 0, xy); double xz = x * z; result.setElement(0, 2, xz); result.setElement(2, 0, xz); double yz = y * z; result.setElement(1, 2, yz); result.setElement(2, 1, yz); }
/** * Sets the rotation matrix, based on the yaw, pitch and roll values. * @param yaw yaw rotation (about a fixed z-axis) * @param pitch pitch rotation (about a fixed y-axis) * @param roll roll rotation (about a fixed x-axis) * @param rotationMatrixToPack the rotation matrix to set, based on the yaw, pitch and roll values */ public static void convertYawPitchRollToMatrix(double yaw, double pitch, double roll, Matrix3d rotationMatrixToPack) { double cosc = Math.cos(yaw); double sinc = Math.sin(yaw); double cosb = Math.cos(pitch); double sinb = Math.sin(pitch); double cosa = Math.cos(roll); double sina = Math.sin(roll); // Introduction to Robotics, 2.64 rotationMatrixToPack.setElement(0, 0, cosc * cosb); rotationMatrixToPack.setElement(0, 1, cosc * sinb * sina - sinc * cosa); rotationMatrixToPack.setElement(0, 2, cosc * sinb * cosa + sinc * sina); rotationMatrixToPack.setElement(1, 0, sinc * cosb); rotationMatrixToPack.setElement(1, 1, sinc * sinb * sina + cosc * cosa); rotationMatrixToPack.setElement(1, 2, sinc * sinb * cosa - cosc * sina); rotationMatrixToPack.setElement(2, 0, -sinb); rotationMatrixToPack.setElement(2, 1, cosb * sina); rotationMatrixToPack.setElement(2, 2, cosb * cosa); }
/** * Converts a vector to tilde form (matrix implementation of cross product) */ public static void toTildeForm(Matrix3d matrixToPack, Tuple3d p) { matrixToPack.setElement(0, 0, 0.0); matrixToPack.setElement(0, 1, -p.getZ()); matrixToPack.setElement(0, 2, p.getY()); matrixToPack.setElement(1, 0, p.getZ()); matrixToPack.setElement(1, 1, 0.0); matrixToPack.setElement(1, 2, -p.getX()); matrixToPack.setElement(2, 0, -p.getY()); matrixToPack.setElement(2, 1, p.getX()); matrixToPack.setElement(2, 2, 0.0); }
public void variableChanged(YoVariable<?> v) { matrix.setElement(i, j, v.getValueAsDouble()); } }
public static void denseMatrixToMatrix3d(DenseMatrix64F denseMatrix, Matrix3d matrixToPack, int startRow, int startColumn) { for (int row = 0; row < 3; row++) { for (int column = 0; column < 3; column++) { matrixToPack.setElement(row, column, denseMatrix.get(row + startRow, column + startColumn)); } } }
public static Matrix3d generateRandomMatrix3d(Random random, double maxAbsolute) { Matrix3d ret = new Matrix3d(); for (int row = 0; row < 3; row++) { for (int column = 0; column < 3; column++) { ret.setElement(row, column, generateRandomDouble(random, maxAbsolute)); } } return ret; }
public Matrix3d getOrientationOffset() { Matrix3d ret = new Matrix3d(); for (int row = 0; row < MATRIX_SIZE; row++) { for (int column = 0; column < MATRIX_SIZE; column++) { String name = getOrientationOffsetPropertyName(row, column); ret.setElement(row, column, getDoubleProperty(name)); } } if (Math.abs(Math.abs(ret.determinant()) - 1.0) > 1e-6) { System.err.println("Not a valid rotation matrix in " + getClass().getName() + ":"); System.err.println(ret); throw new InvalidRotationMatrix(getClass().getName(), ret); } return ret; }
/** * Returns the resulting matrix from vector1*transpose(vector2) * * @param vector1 * @param vector2 */ public static Matrix3d vectorTimesVectorTranspose(Vector3d vector1, Vector3d vector2) { Matrix3d matrix = new Matrix3d(); double[] array1 = new double[3]; double[] array2 = new double[3]; vector1.get(array1); vector2.get(array2); for (int row = 0; row < 3; row++) { for (int column = 0; column < 3; column++) { matrix.setElement(row, column, array1[row] * array2[column]); } } return matrix; }
private void getParameters(ArrayList<Double> time, ArrayList<Vector3d> waypoints, double[] parametersXToPack, double[] parametersYToPack, double[] parametersZToPack) { for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { timeMatrix.setElement(i, j, MathTools.powWithInteger(time.get(i), 2 - j)); } } System.out.println(timeMatrix); timeMatrix.invert(); Vector3d tempVector = new Vector3d(); for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersXToPack[i] = (tempVector.dot(new Vector3d(waypoints.get(0).getX(), waypoints.get(1).getX(), waypoints.get(2).getX()))); } for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersYToPack[i] = (tempVector.dot(new Vector3d(waypoints.get(0).getY(), waypoints.get(1).getY(), waypoints.get(2).getY()))); } for (int i = 0; i < 3; i++) { timeMatrix.getRow(i, tempVector); parametersZToPack[i] = (tempVector.dot(new Vector3d(waypoints.get(0).getZ(), waypoints.get(1).getZ(), waypoints.get(2).getZ()))); } }
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); }
rotationMatrixResultToPack.setElement(0, 0, x.getM00() - 0.5 * (x.getM00() * mx.getM00() + x.getM01() * mx.getM10() + x.getM02() * mx.getM20() - x.getM00())); rotationMatrixResultToPack.setElement(1, 0, x.getM01() - 0.5 * (x.getM00() * mx.getM01() + x.getM01() * mx.getM11() + x.getM02() * mx.getM21() - x.getM01())); rotationMatrixResultToPack.setElement(2, 0, x.getM02() - 0.5 * (x.getM00() * mx.getM02() + x.getM01() * mx.getM12() + x.getM02() * mx.getM22() - x.getM02())); rotationMatrixResultToPack.setElement(0, 1, x.getM10() - 0.5 * (x.getM10() * mx.getM00() + x.getM11() * mx.getM10() + x.getM12() * mx.getM20() - x.getM10())); rotationMatrixResultToPack.setElement(1, 1, x.getM11() - 0.5 * (x.getM10() * mx.getM01() + x.getM11() * mx.getM11() + x.getM12() * mx.getM21() - x.getM11())); rotationMatrixResultToPack.setElement(2, 1, x.getM12() - 0.5 * (x.getM10() * mx.getM02() + x.getM11() * mx.getM12() + x.getM12() * mx.getM22() - x.getM12())); rotationMatrixResultToPack.setElement(0, 2, x.getM20() - 0.5 * (x.getM20() * mx.getM00() + x.getM21() * mx.getM10() + x.getM22() * mx.getM20() - x.getM20())); rotationMatrixResultToPack.setElement(1, 2, x.getM21() - 0.5 * (x.getM20() * mx.getM01() + x.getM21() * mx.getM11() + x.getM22() * mx.getM21() - x.getM21())); rotationMatrixResultToPack.setElement(2, 2, x.getM22() - 0.5 * (x.getM20() * mx.getM02() + x.getM21() * mx.getM12() + x.getM22() * mx.getM22() - x.getM22()));