public void getRotation(double[] yawPitchRoll) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); }
public void getRotation(double[] yawPitchRoll) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, this.yawPitchRoll); yawPitchRoll[0] = 0.0; yawPitchRoll[1] = this.yawPitchRoll[1]; yawPitchRoll[2] = 0.0; }
public void getYawPitchRoll(double[] yawPitchRollToPack) { RotationTools.convertQuaternionToYawPitchRoll(quaternion, yawPitchRollToPack); }
public void getYawPitchRoll(double[] yawPitchRollToPack) { RotationTools.convertQuaternionToYawPitchRoll(this, yawPitchRollToPack); }
public void getRotation(double[] yawPitchRoll) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); }
/** * Compute the pitch rotation from the yaw-pitch-roll rotations induced by the given quaternion. * * <p> The three rotations yaw-pitch-roll need to be computed to get the pitch. If you need more than one of the three rotations, prefer using {@link #convertQuaternionToYawPitchRoll(Quat4d, double[])}. </p> * * @param quaternion unit quaternion from which the pitch is computed. * @return pitch rotation (around the y-axis). */ public static double computePitch(Quat4d quaternion) { double[] yawPitchRoll = yawPitchRollForQuaternionToYawPitchRollConvertor.get(); convertQuaternionToYawPitchRoll(quaternion, yawPitchRoll); return yawPitchRoll[1]; }
/** * Compute the roll rotation from the yaw-pitch-roll rotations induced by the given quaternion. * * <p> The three rotations yaw-pitch-roll need to be computed to get the roll. If you need more than one of the three rotations, prefer using {@link #convertQuaternionToYawPitchRoll(Quat4d, double[])}. </p> * * @param quaternion unit quaternion from which the roll is computed. * @return roll rotation (around the x-axis). */ public static double computeRoll(Quat4d quaternion) { double[] yawPitchRoll = yawPitchRollForQuaternionToYawPitchRollConvertor.get(); convertQuaternionToYawPitchRoll(quaternion, yawPitchRoll); return yawPitchRoll[2]; }
/** * Compute the yaw rotation from the yaw-pitch-roll rotations induced by the given quaternion. * * <p> The three rotations yaw-pitch-roll need to be computed to get the yaw. If you need more than one of the three rotations, prefer using {@link #convertQuaternionToYawPitchRoll(Quat4d, double[])}. </p> * * @param quaternion unit quaternion from which the yaw is computed. * @return yaw rotation (around the z-axis). */ public static double computeYaw(Quat4d quaternion) { double[] yawPitchRoll = yawPitchRollForQuaternionToYawPitchRollConvertor.get(); convertQuaternionToYawPitchRoll(quaternion, yawPitchRoll); return yawPitchRoll[0]; }
public void getRotation(Quat4d rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, rotationToPack); }
private void set(Quat4d newRotation) { RotationTools.convertQuaternionToYawPitchRoll(newRotation, yawPitchRoll); yoFramePose.setYawPitchRoll(yawPitchRoll); }
public void getRotation(Matrix3d rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToMatrix(0.0, yawPitchRoll[1], 0.0, rotationToPack); }
public void getRotation(Quat4f rotationToPack) { RotationTools.convertQuaternionToYawPitchRoll(this.jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, this.jointRotation); rotationToPack.set(jointRotation); }
public void setRotation(double x, double y, double z, double w) { RotationTools.convertQuaternionToYawPitchRoll(x, y, z, w, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
public void setRotation(Quat4d jointRotation) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); RotationTools.convertYawPitchRollToQuaternion(0.0, yawPitchRoll[1], 0.0, this.jointRotation); this.afterJointFrame.setRotation(this.jointRotation); }
public static void convertQuaternionToYawPitchRoll(Quat4d quaternion, double[] yawPitchRollToPack) { convertQuaternionToYawPitchRoll(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getW(), yawPitchRollToPack); }
public static void convertQuaternionToYawPitchRoll(Quat4f quaternion, double[] yawPitchRollToPack) { convertQuaternionToYawPitchRoll(quaternion.getX(), quaternion.getY(), quaternion.getZ(), quaternion.getW(), yawPitchRollToPack); }
@Override public void getConfigurationMatrix(DenseMatrix64F matrix, int rowStart) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); int index = rowStart; matrix.set(index++, 0, yawPitchRoll[1]); matrix.set(index++, 0, jointTranslation.getX()); matrix.set(index++, 0, jointTranslation.getZ()); }
/** * Updates the IMU given the rate gyro inputs. * * @param pqr Matrix Gyro Rate values in order of qd_wy, qd_wx, qd_wz??? */ public void imuUpdate(DenseMatrix64F pqr) { CommonOps.subtractEquals(pqr, bias); unpackQuaternion(q); makeAMatrix(pqr); propagateState(pqr); propagateCovariance(A); /* compute angles from quaternions */ Quat4d quaternion = new Quat4d(); MatrixTools.extractQuat4dFromEJMLVector(quaternion, q, 0); RotationTools.convertQuaternionToYawPitchRoll(quaternion, eulerAngles); }
public String toString() { double[] ypr = new double[3]; RotationTools.convertQuaternionToYawPitchRoll(orientation, ypr); String ret = ("Car= (" + FormattingTools.getFormattedDecimal3D(position.getX()) + "," + FormattingTools.getFormattedDecimal3D(position.getY()) + "," + FormattingTools.getFormattedDecimal3D(position.getZ()) + ")," + " (" + FormattingTools.getFormattedDecimal3D(ypr[0]) + "," + FormattingTools.getFormattedDecimal3D(ypr[1]) + "," + FormattingTools.getFormattedDecimal3D(ypr[2]) + ")"); return ret; }
@Override public void calculateJointStateChecksum(GenericCRC32 checksum) { RotationTools.convertQuaternionToYawPitchRoll(jointRotation, yawPitchRoll); checksum.update(yawPitchRoll[1]); checksum.update(jointTwist.getAngularPartY()); checksum.update(jointAcceleration.getAngularPartY()); checksum.update(jointTranslation.getX()); checksum.update(jointTranslation.getY()); checksum.update(jointTwist.getLinearPartX()); checksum.update(jointTwist.getLinearPartZ()); checksum.update(jointAcceleration.getLinearPartX()); checksum.update(jointAcceleration.getLinearPartZ()); }